Merge "Create a UI for target definitions"
diff --git a/aos/aos_dump_autocomplete.sh b/aos/aos_dump_autocomplete.sh
index c62af99..0f0d73c 100644
--- a/aos/aos_dump_autocomplete.sh
+++ b/aos/aos_dump_autocomplete.sh
@@ -7,3 +7,5 @@
complete -F _aosdump_completions -o default aos_dump.stripped
complete -F _aosdump_completions -o default aos_send
complete -F _aosdump_completions -o default aos_send.stripped
+complete -F _aosdump_completions -o default aos_starter
+complete -F _aosdump_completions -o default aos_starter.stripped
diff --git a/aos/aos_graph_channels.cc b/aos/aos_graph_channels.cc
index e96494a..7eb22ba 100644
--- a/aos/aos_graph_channels.cc
+++ b/aos/aos_graph_channels.cc
@@ -185,7 +185,7 @@
// Now generate graphvis compatible output.
std::stringstream graph_out;
graph_out << "digraph g {" << std::endl;
- for (const std::pair<const aos::Channel *, ChannelConnections> &c :
+ for (const std::pair<const aos::Channel *const, ChannelConnections> &c :
connections) {
const std::string channel = absl::StrCat(
c.first->name()->string_view(), "\n", c.first->type()->string_view());
diff --git a/aos/events/event_loop.cc b/aos/events/event_loop.cc
index 8728111..68cc923 100644
--- a/aos/events/event_loop.cc
+++ b/aos/events/event_loop.cc
@@ -31,6 +31,15 @@
RawSender::~RawSender() { event_loop_->DeleteSender(this); }
+bool RawSender::DoSend(const SharedSpan data,
+ monotonic_clock::time_point monotonic_remote_time,
+ realtime_clock::time_point realtime_remote_time,
+ uint32_t remote_queue_index,
+ const UUID &source_boot_uuid) {
+ return DoSend(data->data(), data->size(), monotonic_remote_time,
+ realtime_remote_time, remote_queue_index, source_boot_uuid);
+}
+
RawFetcher::RawFetcher(EventLoop *event_loop, const Channel *channel)
: event_loop_(event_loop),
channel_(channel),
diff --git a/aos/events/event_loop.h b/aos/events/event_loop.h
index 1000703..0e7b66a 100644
--- a/aos/events/event_loop.h
+++ b/aos/events/event_loop.h
@@ -136,6 +136,8 @@
// and as a building block to implement typed senders.
class RawSender {
public:
+ using SharedSpan = std::shared_ptr<const absl::Span<const uint8_t>>;
+
RawSender(EventLoop *event_loop, const Channel *channel);
RawSender(const RawSender &) = delete;
RawSender &operator=(const RawSender &) = delete;
@@ -164,6 +166,15 @@
realtime_clock::time_point realtime_remote_time,
uint32_t remote_queue_index, const UUID &source_boot_uuid);
+ // Sends a single block of data by refcounting it to avoid copies. The data
+ // must not change after being passed into Send. The remote arguments have the
+ // same meaning as in Send above.
+ bool Send(const SharedSpan data);
+ bool Send(const SharedSpan data,
+ monotonic_clock::time_point monotonic_remote_time,
+ realtime_clock::time_point realtime_remote_time,
+ uint32_t remote_queue_index, const UUID &remote_boot_uuid);
+
const Channel *channel() const { return channel_; }
// Returns the time_points that the last message was sent at.
@@ -209,6 +220,11 @@
realtime_clock::time_point realtime_remote_time,
uint32_t remote_queue_index,
const UUID &source_boot_uuid) = 0;
+ virtual bool DoSend(const SharedSpan data,
+ monotonic_clock::time_point monotonic_remote_time,
+ realtime_clock::time_point realtime_remote_time,
+ uint32_t remote_queue_index,
+ const UUID &source_boot_uuid);
EventLoop *const event_loop_;
const Channel *const channel_;
diff --git a/aos/events/event_loop_tmpl.h b/aos/events/event_loop_tmpl.h
index 698191e..7e560fe 100644
--- a/aos/events/event_loop_tmpl.h
+++ b/aos/events/event_loop_tmpl.h
@@ -36,8 +36,9 @@
template <typename Watch>
void EventLoop::MakeWatcher(const std::string_view channel_name, Watch &&w) {
- using MessageType = typename event_loop_internal::watch_message_type_trait<
- decltype(&Watch::operator())>::message_type;
+ using MessageType =
+ typename event_loop_internal::watch_message_type_trait<decltype(
+ &Watch::operator())>::message_type;
const Channel *channel = configuration::GetChannel(
configuration_, channel_name, MessageType::GetFullyQualifiedName(),
name(), node());
@@ -174,6 +175,31 @@
return false;
}
+inline bool RawSender::Send(const SharedSpan data) {
+ return Send(std::move(data), monotonic_clock::min_time,
+ realtime_clock::min_time, 0xffffffffu, event_loop_->boot_uuid());
+}
+
+inline bool RawSender::Send(
+ const SharedSpan data,
+ aos::monotonic_clock::time_point monotonic_remote_time,
+ aos::realtime_clock::time_point realtime_remote_time,
+ uint32_t remote_queue_index, const UUID &uuid) {
+ const size_t size = data->size();
+ if (DoSend(std::move(data), monotonic_remote_time, realtime_remote_time,
+ remote_queue_index, uuid)) {
+ timing_.size.Add(size);
+ timing_.sender->mutate_count(timing_.sender->count() + 1);
+ ftrace_.FormatMessage(
+ "%.*s: sent shared: event=%" PRId64 " queue=%" PRIu32,
+ static_cast<int>(ftrace_prefix_.size()), ftrace_prefix_.data(),
+ static_cast<int64_t>(monotonic_sent_time().time_since_epoch().count()),
+ sent_queue_index());
+ return true;
+ }
+ return false;
+}
+
inline monotonic_clock::time_point TimerHandler::Call(
std::function<monotonic_clock::time_point()> get_time,
monotonic_clock::time_point event_time) {
diff --git a/aos/events/logging/log_cat.cc b/aos/events/logging/log_cat.cc
index 71293af..60ad0e2 100644
--- a/aos/events/logging/log_cat.cc
+++ b/aos/events/logging/log_cat.cc
@@ -20,6 +20,7 @@
DEFINE_string(type, "",
"Channel type to match for printing out channels. Empty means no "
"type filter.");
+DEFINE_bool(json, false, "If true, print fully valid JSON");
DEFINE_bool(fetch, false,
"If true, also print out the messages from before the start of the "
"log file");
@@ -46,6 +47,30 @@
DEFINE_bool(channels, false,
"If true, print out all the configured channels for this log.");
+using aos::monotonic_clock;
+namespace chrono = std::chrono;
+
+void StreamSeconds(std::ostream &stream,
+ const aos::monotonic_clock::time_point now) {
+ if (now < monotonic_clock::epoch()) {
+ chrono::seconds seconds =
+ chrono::duration_cast<chrono::seconds>(now.time_since_epoch());
+
+ stream << "-" << -seconds.count() << "." << std::setfill('0')
+ << std::setw(9)
+ << chrono::duration_cast<chrono::nanoseconds>(seconds -
+ now.time_since_epoch())
+ .count();
+ } else {
+ chrono::seconds seconds =
+ chrono::duration_cast<chrono::seconds>(now.time_since_epoch());
+ stream << seconds.count() << "." << std::setfill('0') << std::setw(9)
+ << chrono::duration_cast<chrono::nanoseconds>(
+ now.time_since_epoch() - seconds)
+ .count();
+ }
+}
+
// Print the flatbuffer out to stdout, both to remove the unnecessary cruft from
// glog and to allow the user to readily redirect just the logged output
// independent of any debugging information on stderr.
@@ -64,18 +89,43 @@
builder, channel->schema(), static_cast<const uint8_t *>(context.data),
{FLAGS_pretty, static_cast<size_t>(FLAGS_max_vector_size)});
- if (context.monotonic_remote_time != context.monotonic_event_time) {
- std::cout << node_name << context.realtime_event_time << " ("
- << context.monotonic_event_time << ") sent "
- << context.realtime_remote_time << " ("
- << context.monotonic_remote_time << ") "
- << channel->name()->c_str() << ' ' << channel->type()->c_str()
- << ": " << *builder << std::endl;
+ if (FLAGS_json) {
+ std::cout << "{";
+ if (!node_name.empty()) {
+ std::cout << "\"node\": \"" << node_name << "\", ";
+ }
+ std::cout << "\"monotonic_event_time\": ";
+ StreamSeconds(std::cout, context.monotonic_event_time);
+ std::cout << ", \"realtime_event_time\": \"" << context.realtime_event_time
+ << "\", ";
+
+ if (context.monotonic_remote_time != context.monotonic_event_time) {
+ std::cout << "\"monotonic_remote_time\": ";
+ StreamSeconds(std::cout, context.monotonic_remote_time);
+ std::cout << ", \"realtime_remote_time\": \""
+ << context.realtime_remote_time << "\", ";
+ }
+
+ std::cout << "\"channel\": "
+ << aos::configuration::StrippedChannelToString(channel)
+ << ", \"data\": " << *builder << "}" << std::endl;
} else {
- std::cout << node_name << context.realtime_event_time << " ("
- << context.monotonic_event_time << ") "
- << channel->name()->c_str() << ' ' << channel->type()->c_str()
- << ": " << *builder << std::endl;
+ if (!node_name.empty()) {
+ std::cout << node_name << " ";
+ }
+ if (context.monotonic_remote_time != context.monotonic_event_time) {
+ std::cout << context.realtime_event_time << " ("
+ << context.monotonic_event_time << ") sent "
+ << context.realtime_remote_time << " ("
+ << context.monotonic_remote_time << ") "
+ << channel->name()->c_str() << ' ' << channel->type()->c_str()
+ << ": " << *builder << std::endl;
+ } else {
+ std::cout << context.realtime_event_time << " ("
+ << context.monotonic_event_time << ") "
+ << channel->name()->c_str() << ' ' << channel->type()->c_str()
+ << ": " << *builder << std::endl;
+ }
}
}
@@ -214,7 +264,7 @@
node_name_(
event_loop_->node() == nullptr
? ""
- : std::string(event_loop->node()->name()->string_view()) + " "),
+ : std::string(event_loop->node()->name()->string_view())),
builder_(builder) {
event_loop_->SkipTimingReport();
event_loop_->SkipAosLog();
@@ -259,6 +309,9 @@
void SetStarted(bool started, aos::monotonic_clock::time_point monotonic_now,
aos::realtime_clock::time_point realtime_now) {
started_ = started;
+ if (FLAGS_json) {
+ return;
+ }
if (started_) {
std::cout << std::endl;
std::cout << (event_loop_->node() != nullptr
diff --git a/aos/events/logging/log_reader.cc b/aos/events/logging/log_reader.cc
index d6213ae..a80b0e5 100644
--- a/aos/events/logging/log_reader.cc
+++ b/aos/events/logging/log_reader.cc
@@ -706,7 +706,7 @@
state->monotonic_start_time(
timestamped_message.monotonic_event_time.boot) ||
event_loop_factory_ != nullptr) {
- if (timestamped_message.data.span().size() != 0u) {
+ if (timestamped_message.data != nullptr) {
if (timestamped_message.monotonic_remote_time !=
BootTimestamp::min_time()) {
// Confirm that the message was sent on the sending node before the
@@ -851,7 +851,7 @@
// through events like this, they can set
// --skip_missing_forwarding_entries or ignore_missing_data_.
CHECK_LT(next.channel_index, last_message.size());
- if (next.data.span().size() == 0u) {
+ if (next.data == nullptr) {
last_message[next.channel_index] = true;
} else {
if (last_message[next.channel_index]) {
@@ -874,9 +874,7 @@
.count()
<< " start "
<< monotonic_start_time().time_since_epoch().count() << " "
- << FlatbufferToJson(
- timestamped_message.data,
- {.multi_line = false, .max_vector_size = 100});
+ << *timestamped_message.data;
}
const BootTimestamp next_time = state->OldestMessageTime();
@@ -1421,8 +1419,8 @@
// Send! Use the replayed queue index here instead of the logged queue index
// for the remote queue index. This makes re-logging work.
const bool sent = sender->Send(
- timestamped_message.data.message().data()->Data(),
- timestamped_message.data.message().data()->size(),
+ RawSender::SharedSpan(timestamped_message.data,
+ ×tamped_message.data->span),
timestamped_message.monotonic_remote_time.time,
timestamped_message.realtime_remote_time, remote_queue_index,
(channel_source_state_[timestamped_message.channel_index] != nullptr
diff --git a/aos/events/logging/logfile_utils.cc b/aos/events/logging/logfile_utils.cc
index 73578e6..dd82418 100644
--- a/aos/events/logging/logfile_utils.cc
+++ b/aos/events/logging/logfile_utils.cc
@@ -43,9 +43,20 @@
"last header as the actual header.");
namespace aos::logger {
+namespace {
namespace chrono = std::chrono;
+template <typename T>
+void PrintOptionalOrNull(std::ostream *os, const std::optional<T> &t) {
+ if (t.has_value()) {
+ *os << *t;
+ } else {
+ *os << "null";
+ }
+}
+} // namespace
+
DetachedBufferWriter::DetachedBufferWriter(
std::string_view filename, std::unique_ptr<DetachedBufferEncoder> encoder)
: filename_(filename), encoder_(std::move(encoder)) {
@@ -499,23 +510,81 @@
<< FlatbufferToJson(log_file_header()->node());
}
-std::optional<SizePrefixedFlatbufferVector<MessageHeader>>
-MessageReader::ReadMessage() {
+std::shared_ptr<UnpackedMessageHeader> MessageReader::ReadMessage() {
absl::Span<const uint8_t> msg_data = span_reader_.ReadMessage();
if (msg_data == absl::Span<const uint8_t>()) {
- return std::nullopt;
+ return nullptr;
}
- SizePrefixedFlatbufferVector<MessageHeader> result(msg_data);
+ SizePrefixedFlatbufferSpan<MessageHeader> msg(msg_data);
+ CHECK(msg.Verify()) << ": Corrupted message from " << filename();
- CHECK(result.Verify()) << ": Corrupted message from " << filename();
+ auto result = UnpackedMessageHeader::MakeMessage(msg.message());
- const monotonic_clock::time_point timestamp = monotonic_clock::time_point(
- chrono::nanoseconds(result.message().monotonic_sent_time()));
+ const monotonic_clock::time_point timestamp = result->monotonic_sent_time;
newest_timestamp_ = std::max(newest_timestamp_, timestamp);
- VLOG(2) << "Read from " << filename() << " data " << FlatbufferToJson(result);
- return std::move(result);
+ VLOG(2) << "Read from " << filename() << " data " << FlatbufferToJson(msg);
+ return result;
+}
+
+std::shared_ptr<UnpackedMessageHeader> UnpackedMessageHeader::MakeMessage(
+ const MessageHeader &message) {
+ const size_t data_size = message.has_data() ? message.data()->size() : 0;
+
+ UnpackedMessageHeader *const unpacked_message =
+ reinterpret_cast<UnpackedMessageHeader *>(
+ malloc(sizeof(UnpackedMessageHeader) + data_size +
+ kChannelDataAlignment - 1));
+
+ CHECK(message.has_channel_index());
+ CHECK(message.has_monotonic_sent_time());
+
+ absl::Span<uint8_t> span;
+ if (data_size > 0) {
+ span =
+ absl::Span<uint8_t>(reinterpret_cast<uint8_t *>(RoundChannelData(
+ &unpacked_message->actual_data[0], data_size)),
+ data_size);
+ }
+
+ std::optional<std::chrono::nanoseconds> monotonic_remote_time;
+ if (message.has_monotonic_remote_time()) {
+ monotonic_remote_time =
+ std::chrono::nanoseconds(message.monotonic_remote_time());
+ }
+ std::optional<realtime_clock::time_point> realtime_remote_time;
+ if (message.has_realtime_remote_time()) {
+ realtime_remote_time = realtime_clock::time_point(
+ chrono::nanoseconds(message.realtime_remote_time()));
+ }
+
+ std::optional<uint32_t> remote_queue_index;
+ if (message.has_remote_queue_index()) {
+ remote_queue_index = message.remote_queue_index();
+ }
+
+ new (unpacked_message) UnpackedMessageHeader{
+ .channel_index = message.channel_index(),
+ .monotonic_sent_time = monotonic_clock::time_point(
+ chrono::nanoseconds(message.monotonic_sent_time())),
+ .realtime_sent_time = realtime_clock::time_point(
+ chrono::nanoseconds(message.realtime_sent_time())),
+ .queue_index = message.queue_index(),
+ .monotonic_remote_time = monotonic_remote_time,
+ .realtime_remote_time = realtime_remote_time,
+ .remote_queue_index = remote_queue_index,
+ .monotonic_timestamp_time = monotonic_clock::time_point(
+ std::chrono::nanoseconds(message.monotonic_timestamp_time())),
+ .has_monotonic_timestamp_time = message.has_monotonic_timestamp_time(),
+ .span = span};
+
+ if (data_size > 0) {
+ memcpy(span.data(), message.data()->data(), data_size);
+ }
+
+ return std::shared_ptr<UnpackedMessageHeader>(unpacked_message,
+ &DestroyAndFree);
}
PartsMessageReader::PartsMessageReader(LogParts log_parts)
@@ -569,19 +638,19 @@
}
}
-std::optional<SizePrefixedFlatbufferVector<MessageHeader>>
-PartsMessageReader::ReadMessage() {
+std::shared_ptr<UnpackedMessageHeader> PartsMessageReader::ReadMessage() {
while (!done_) {
- std::optional<SizePrefixedFlatbufferVector<MessageHeader>> message =
+ std::shared_ptr<UnpackedMessageHeader> message =
message_reader_.ReadMessage();
if (message) {
newest_timestamp_ = message_reader_.newest_timestamp();
- const monotonic_clock::time_point monotonic_sent_time(
- chrono::nanoseconds(message->message().monotonic_sent_time()));
- // TODO(austin): Does this work with startup? Might need to use the start
- // time.
- // TODO(austin): Does this work with startup when we don't know the remote
- // start time too? Look at one of those logs to compare.
+ const monotonic_clock::time_point monotonic_sent_time =
+ message->monotonic_sent_time;
+
+ // TODO(austin): Does this work with startup? Might need to use the
+ // start time.
+ // TODO(austin): Does this work with startup when we don't know the
+ // remote start time too? Look at one of those logs to compare.
if (monotonic_sent_time >
parts_.monotonic_start_time + max_out_of_order_duration()) {
after_start_ = true;
@@ -599,7 +668,7 @@
NextLog();
}
newest_timestamp_ = monotonic_clock::max_time;
- return std::nullopt;
+ return nullptr;
}
void PartsMessageReader::NextLog() {
@@ -645,13 +714,29 @@
channel_index == m2.channel_index && queue_index == m2.queue_index;
}
+std::ostream &operator<<(std::ostream &os, const UnpackedMessageHeader &m) {
+ os << "{.channel_index=" << m.channel_index
+ << ", .monotonic_sent_time=" << m.monotonic_sent_time
+ << ", .realtime_sent_time=" << m.realtime_sent_time
+ << ", .queue_index=" << m.queue_index;
+ if (m.monotonic_remote_time) {
+ os << ", .monotonic_remote_time=" << m.monotonic_remote_time->count();
+ }
+ os << ", .realtime_remote_time=";
+ PrintOptionalOrNull(&os, m.realtime_remote_time);
+ os << ", .remote_queue_index=";
+ PrintOptionalOrNull(&os, m.remote_queue_index);
+ if (m.has_monotonic_timestamp_time) {
+ os << ", .monotonic_timestamp_time=" << m.monotonic_timestamp_time;
+ }
+ return os;
+}
+
std::ostream &operator<<(std::ostream &os, const Message &m) {
os << "{.channel_index=" << m.channel_index
<< ", .queue_index=" << m.queue_index << ", .timestamp=" << m.timestamp;
- if (m.data.Verify()) {
- os << ", .data="
- << aos::FlatbufferToJson(m.data,
- {.multi_line = false, .max_vector_size = 1});
+ if (m.data != nullptr) {
+ os << ", .data=" << m;
}
os << "}";
return os;
@@ -674,10 +759,8 @@
if (m.monotonic_timestamp_time != BootTimestamp::min_time()) {
os << ", .monotonic_timestamp_time=" << m.monotonic_timestamp_time;
}
- if (m.data.Verify()) {
- os << ", .data="
- << aos::FlatbufferToJson(m.data,
- {.multi_line = false, .max_vector_size = 1});
+ if (m.data != nullptr) {
+ os << ", .data=" << *m.data;
}
os << "}";
return os;
@@ -700,7 +783,7 @@
break;
}
- std::optional<SizePrefixedFlatbufferVector<MessageHeader>> m =
+ std::shared_ptr<UnpackedMessageHeader> m =
parts_message_reader_.ReadMessage();
// No data left, sorted forever, work through what is left.
if (!m) {
@@ -709,36 +792,32 @@
}
size_t monotonic_timestamp_boot = 0;
- if (m.value().message().has_monotonic_timestamp_time()) {
+ if (m->has_monotonic_timestamp_time) {
monotonic_timestamp_boot = parts().logger_boot_count;
}
size_t monotonic_remote_boot = 0xffffff;
- if (m.value().message().has_monotonic_remote_time()) {
+ if (m->monotonic_remote_time.has_value()) {
const Node *node = parts().config->nodes()->Get(
- source_node_index_[m->message().channel_index()]);
+ source_node_index_[m->channel_index]);
std::optional<size_t> boot = parts_message_reader_.boot_count(
- source_node_index_[m->message().channel_index()]);
+ source_node_index_[m->channel_index]);
CHECK(boot) << ": Failed to find boot for node " << MaybeNodeName(node)
<< ", with index "
- << source_node_index_[m->message().channel_index()];
+ << source_node_index_[m->channel_index];
monotonic_remote_boot = *boot;
}
- messages_.insert(Message{
- .channel_index = m.value().message().channel_index(),
- .queue_index =
- BootQueueIndex{.boot = parts().boot_count,
- .index = m.value().message().queue_index()},
- .timestamp =
- BootTimestamp{
- .boot = parts().boot_count,
- .time = monotonic_clock::time_point(std::chrono::nanoseconds(
- m.value().message().monotonic_sent_time()))},
- .monotonic_remote_boot = monotonic_remote_boot,
- .monotonic_timestamp_boot = monotonic_timestamp_boot,
- .data = std::move(m.value())});
+ messages_.insert(
+ Message{.channel_index = m->channel_index,
+ .queue_index = BootQueueIndex{.boot = parts().boot_count,
+ .index = m->queue_index},
+ .timestamp = BootTimestamp{.boot = parts().boot_count,
+ .time = m->monotonic_sent_time},
+ .monotonic_remote_boot = monotonic_remote_boot,
+ .monotonic_timestamp_boot = monotonic_timestamp_boot,
+ .data = std::move(m)});
// Now, update sorted_until_ to match the new message.
if (parts_message_reader_.newest_timestamp() >
@@ -878,17 +957,17 @@
oldest = m;
current_ = &parts_sorter;
} else if (*m == *oldest) {
- // Found a duplicate. If there is a choice, we want the one which has the
- // timestamp time.
- if (!m->data.message().has_monotonic_timestamp_time()) {
+ // Found a duplicate. If there is a choice, we want the one which has
+ // the timestamp time.
+ if (!m->data->has_monotonic_timestamp_time) {
parts_sorter.PopFront();
- } else if (!oldest->data.message().has_monotonic_timestamp_time()) {
+ } else if (!oldest->data->has_monotonic_timestamp_time) {
current_->PopFront();
current_ = &parts_sorter;
oldest = m;
} else {
- CHECK_EQ(m->data.message().monotonic_timestamp_time(),
- oldest->data.message().monotonic_timestamp_time());
+ CHECK_EQ(m->data->monotonic_timestamp_time,
+ oldest->data->monotonic_timestamp_time);
parts_sorter.PopFront();
}
}
@@ -1017,7 +1096,7 @@
CHECK_LT(timestamp_mapper->node(), nodes_data_.size());
NodeData *node_data = &nodes_data_[timestamp_mapper->node()];
- // Only set it if this node delivers to the peer timestamp_mapper. Otherwise
+ // Only set it if this node delivers to the peer timestamp_mapper. Otherwise
// we could needlessly save data.
if (node_data->any_delivered) {
VLOG(1) << "Registering on node " << node() << " for peer node "
@@ -1035,8 +1114,7 @@
.channel_index = m->channel_index,
.queue_index = m->queue_index,
.monotonic_event_time = m->timestamp,
- .realtime_event_time = aos::realtime_clock::time_point(
- std::chrono::nanoseconds(m->data.message().realtime_sent_time())),
+ .realtime_event_time = m->data->realtime_sent_time,
.remote_queue_index = BootQueueIndex::Invalid(),
.monotonic_remote_time = BootTimestamp::min_time(),
.realtime_remote_time = realtime_clock::min_time,
@@ -1086,16 +1164,17 @@
return true;
}
- // We need to only add messages to the list so they get processed for messages
- // which are delivered. Reuse the flow below which uses messages_ by just
- // adding the new message to messages_ and continuing.
+ // We need to only add messages to the list so they get processed for
+ // messages which are delivered. Reuse the flow below which uses messages_
+ // by just adding the new message to messages_ and continuing.
if (messages_.empty()) {
if (!Queue()) {
// Found nothing to add, we are out of data!
return false;
}
- // Now that it has been added (and cannibalized), forget about it upstream.
+ // Now that it has been added (and cannibalized), forget about it
+ // upstream.
boot_merger_.PopFront();
}
@@ -1110,7 +1189,8 @@
timestamp_callback_(&matched_messages_.back());
return true;
} else {
- // Got a timestamp, find the matching remote data, match it, and return it.
+ // Got a timestamp, find the matching remote data, match it, and return
+ // it.
Message data = MatchingMessageFor(*m);
// Return the data from the remote. The local message only has timestamp
@@ -1119,21 +1199,16 @@
.channel_index = m->channel_index,
.queue_index = m->queue_index,
.monotonic_event_time = m->timestamp,
- .realtime_event_time = aos::realtime_clock::time_point(
- std::chrono::nanoseconds(m->data.message().realtime_sent_time())),
+ .realtime_event_time = m->data->realtime_sent_time,
.remote_queue_index =
BootQueueIndex{.boot = m->monotonic_remote_boot,
- .index = m->data.message().remote_queue_index()},
- .monotonic_remote_time =
- {m->monotonic_remote_boot,
- monotonic_clock::time_point(std::chrono::nanoseconds(
- m->data.message().monotonic_remote_time()))},
- .realtime_remote_time = realtime_clock::time_point(
- std::chrono::nanoseconds(m->data.message().realtime_remote_time())),
- .monotonic_timestamp_time =
- {m->monotonic_timestamp_boot,
- monotonic_clock::time_point(std::chrono::nanoseconds(
- m->data.message().monotonic_timestamp_time()))},
+ .index = m->data->remote_queue_index.value()},
+ .monotonic_remote_time = {m->monotonic_remote_boot,
+ monotonic_clock::time_point(
+ m->data->monotonic_remote_time.value())},
+ .realtime_remote_time = m->data->realtime_remote_time.value(),
+ .monotonic_timestamp_time = {m->monotonic_timestamp_boot,
+ m->data->monotonic_timestamp_time},
.data = std::move(data.data)});
CHECK_GE(matched_messages_.back().monotonic_event_time, last_message_time_);
last_message_time_ = matched_messages_.back().monotonic_event_time;
@@ -1153,8 +1228,9 @@
}
void TimestampMapper::QueueFor(chrono::nanoseconds time_estimation_buffer) {
- // Note: queueing for time doesn't really work well across boots. So we just
- // assume that if you are using this, you only care about the current boot.
+ // Note: queueing for time doesn't really work well across boots. So we
+ // just assume that if you are using this, you only care about the current
+ // boot.
//
// TODO(austin): Is that the right concept?
//
@@ -1191,36 +1267,37 @@
Message TimestampMapper::MatchingMessageFor(const Message &message) {
// Figure out what queue index we are looking for.
- CHECK(message.data.message().has_remote_queue_index());
+ CHECK_NOTNULL(message.data);
+ CHECK(message.data->remote_queue_index.has_value());
const BootQueueIndex remote_queue_index =
BootQueueIndex{.boot = message.monotonic_remote_boot,
- .index = message.data.message().remote_queue_index()};
+ .index = *message.data->remote_queue_index};
- CHECK(message.data.message().has_monotonic_remote_time());
- CHECK(message.data.message().has_realtime_remote_time());
+ CHECK(message.data->monotonic_remote_time.has_value());
+ CHECK(message.data->realtime_remote_time.has_value());
const BootTimestamp monotonic_remote_time{
.boot = message.monotonic_remote_boot,
- .time = monotonic_clock::time_point(std::chrono::nanoseconds(
- message.data.message().monotonic_remote_time()))};
- const realtime_clock::time_point realtime_remote_time(
- std::chrono::nanoseconds(message.data.message().realtime_remote_time()));
+ .time = monotonic_clock::time_point(
+ message.data->monotonic_remote_time.value())};
+ const realtime_clock::time_point realtime_remote_time =
+ *message.data->realtime_remote_time;
- TimestampMapper *peer = nodes_data_[source_node_[message.channel_index]].peer;
+ TimestampMapper *peer =
+ nodes_data_[source_node_[message.data->channel_index]].peer;
// We only register the peers which we have data for. So, if we are being
- // asked to pull a timestamp from a peer which doesn't exist, return an empty
- // message.
+ // asked to pull a timestamp from a peer which doesn't exist, return an
+ // empty message.
if (peer == nullptr) {
// TODO(austin): Make sure the tests hit all these paths with a boot count
// of 1...
- return Message{
- .channel_index = message.channel_index,
- .queue_index = remote_queue_index,
- .timestamp = monotonic_remote_time,
- .monotonic_remote_boot = 0xffffff,
- .monotonic_timestamp_boot = 0xffffff,
- .data = SizePrefixedFlatbufferVector<MessageHeader>::Empty()};
+ return Message{.channel_index = message.channel_index,
+ .queue_index = remote_queue_index,
+ .timestamp = monotonic_remote_time,
+ .monotonic_remote_boot = 0xffffff,
+ .monotonic_timestamp_boot = 0xffffff,
+ .data = nullptr};
}
// The queue which will have the matching data, if available.
@@ -1230,13 +1307,12 @@
peer->QueueUnmatchedUntil(monotonic_remote_time);
if (data_queue->empty()) {
- return Message{
- .channel_index = message.channel_index,
- .queue_index = remote_queue_index,
- .timestamp = monotonic_remote_time,
- .monotonic_remote_boot = 0xffffff,
- .monotonic_timestamp_boot = 0xffffff,
- .data = SizePrefixedFlatbufferVector<MessageHeader>::Empty()};
+ return Message{.channel_index = message.channel_index,
+ .queue_index = remote_queue_index,
+ .timestamp = monotonic_remote_time,
+ .monotonic_remote_boot = 0xffffff,
+ .monotonic_timestamp_boot = 0xffffff,
+ .data = nullptr};
}
if (remote_queue_index < data_queue->front().queue_index ||
@@ -1247,7 +1323,7 @@
.timestamp = monotonic_remote_time,
.monotonic_remote_boot = 0xffffff,
.monotonic_timestamp_boot = 0xffffff,
- .data = SizePrefixedFlatbufferVector<MessageHeader>::Empty()};
+ .data = nullptr};
}
// The algorithm below is constant time with some assumptions. We need there
@@ -1267,8 +1343,7 @@
CHECK_EQ(result.timestamp, monotonic_remote_time)
<< ": Queue index matches, but timestamp doesn't. Please investigate!";
- CHECK_EQ(realtime_clock::time_point(std::chrono::nanoseconds(
- result.data.message().realtime_sent_time())),
+ CHECK_EQ(result.data->realtime_sent_time,
realtime_remote_time)
<< ": Queue index matches, but timestamp doesn't. Please investigate!";
// Now drop the data off the front. We have deduplicated timestamps, so we
@@ -1288,23 +1363,22 @@
m.timestamp.boot == remote_boot;
});
if (it == data_queue->end()) {
- return Message{
- .channel_index = message.channel_index,
- .queue_index = remote_queue_index,
- .timestamp = monotonic_remote_time,
- .monotonic_remote_boot = 0xffffff,
- .monotonic_timestamp_boot = 0xffffff,
- .data = SizePrefixedFlatbufferVector<MessageHeader>::Empty()};
+ return Message{.channel_index = message.channel_index,
+ .queue_index = remote_queue_index,
+ .timestamp = monotonic_remote_time,
+ .monotonic_remote_boot = 0xffffff,
+ .monotonic_timestamp_boot = 0xffffff,
+ .data = nullptr};
}
Message result = std::move(*it);
CHECK_EQ(result.timestamp, monotonic_remote_time)
- << ": Queue index matches, but timestamp doesn't. Please investigate!";
- CHECK_EQ(realtime_clock::time_point(std::chrono::nanoseconds(
- result.data.message().realtime_sent_time())),
- realtime_remote_time)
- << ": Queue index matches, but timestamp doesn't. Please investigate!";
+ << ": Queue index matches, but timestamp doesn't. Please "
+ "investigate!";
+ CHECK_EQ(result.data->realtime_sent_time, realtime_remote_time)
+ << ": Queue index matches, but timestamp doesn't. Please "
+ "investigate!";
// TODO(austin): We still go in order, so we can erase from the beginning to
// our iterator minus 1. That'll keep 1 in the queue.
@@ -1330,7 +1404,8 @@
return;
}
- // Now that it has been added (and cannibalized), forget about it upstream.
+ // Now that it has been added (and cannibalized), forget about it
+ // upstream.
boot_merger_.PopFront();
}
}
@@ -1346,8 +1421,8 @@
if (node_data.channels[m->channel_index].delivered) {
// TODO(austin): This copies the data... Probably not worth stressing
// about yet.
- // TODO(austin): Bound how big this can get. We tend not to send massive
- // data, so we can probably ignore this for a bit.
+ // TODO(austin): Bound how big this can get. We tend not to send
+ // massive data, so we can probably ignore this for a bit.
node_data.channels[m->channel_index].messages.emplace_back(*m);
}
}
diff --git a/aos/events/logging/logfile_utils.h b/aos/events/logging/logfile_utils.h
index 5c5f709..4c82e4d 100644
--- a/aos/events/logging/logfile_utils.h
+++ b/aos/events/logging/logfile_utils.h
@@ -253,6 +253,8 @@
std::optional<SizePrefixedFlatbufferVector<MessageHeader>> ReadNthMessage(
std::string_view filename, size_t n);
+class UnpackedMessageHeader;
+
// Class which handles reading the header and messages from the log file. This
// handles any per-file state left before merging below.
class MessageReader {
@@ -284,7 +286,7 @@
}
// Returns the next message if there is one.
- std::optional<SizePrefixedFlatbufferVector<MessageHeader>> ReadMessage();
+ std::shared_ptr<UnpackedMessageHeader> ReadMessage();
// The time at which we need to read another chunk from the logfile.
monotonic_clock::time_point queue_data_time() const {
@@ -334,7 +336,7 @@
// Returns the next message if there is one, or nullopt if we have reached the
// end of all the files.
// Note: reading the next message may change the max_out_of_order_duration().
- std::optional<SizePrefixedFlatbufferVector<MessageHeader>> ReadMessage();
+ std::shared_ptr<UnpackedMessageHeader> ReadMessage();
// Returns the boot count for the requested node, or std::nullopt if we don't
// know.
@@ -377,6 +379,54 @@
std::vector<std::optional<size_t>> boot_counts_;
};
+// Stores MessageHeader as a flat header and inline, aligned block of data.
+class UnpackedMessageHeader {
+ public:
+ UnpackedMessageHeader(const UnpackedMessageHeader &) = delete;
+ UnpackedMessageHeader &operator=(const UnpackedMessageHeader &) = delete;
+
+ // The channel.
+ uint32_t channel_index = 0xffffffff;
+
+ monotonic_clock::time_point monotonic_sent_time;
+ realtime_clock::time_point realtime_sent_time;
+
+ // The local queue index.
+ uint32_t queue_index = 0xffffffff;
+
+ std::optional<std::chrono::nanoseconds> monotonic_remote_time;
+
+ std::optional<realtime_clock::time_point> realtime_remote_time;
+ std::optional<uint32_t> remote_queue_index;
+
+ // This field is defaulted in the flatbuffer, so we need to store both the
+ // possibly defaulted value and whether it is defaulted.
+ monotonic_clock::time_point monotonic_timestamp_time;
+ bool has_monotonic_timestamp_time;
+
+ static std::shared_ptr<UnpackedMessageHeader> MakeMessage(
+ const MessageHeader &message);
+
+ // Note: we are storing a span here because we need something to put in the
+ // SharedSpan pointer that RawSender takes. We are using the aliasing
+ // constructor of shared_ptr to avoid the allocation, and it needs a nice
+ // pointer to track.
+ absl::Span<const uint8_t> span;
+
+ char actual_data[];
+
+ private:
+ ~UnpackedMessageHeader() {}
+
+ static void DestroyAndFree(UnpackedMessageHeader *p) {
+ p->~UnpackedMessageHeader();
+ free(p);
+ }
+};
+
+std::ostream &operator<<(std::ostream &os,
+ const UnpackedMessageHeader &message);
+
// Struct to hold a message as it gets sorted on a single node.
struct Message {
// The channel.
@@ -394,8 +444,7 @@
size_t monotonic_timestamp_boot = 0xffffff;
- // The data (either a timestamp header, or a data header).
- SizePrefixedFlatbufferVector<MessageHeader> data;
+ std::shared_ptr<UnpackedMessageHeader> data;
bool operator<(const Message &m2) const;
bool operator>=(const Message &m2) const;
@@ -420,7 +469,7 @@
BootTimestamp monotonic_timestamp_time;
- SizePrefixedFlatbufferVector<MessageHeader> data;
+ std::shared_ptr<UnpackedMessageHeader> data;
};
std::ostream &operator<<(std::ostream &os, const TimestampedMessage &m);
diff --git a/aos/events/logging/logfile_utils_test.cc b/aos/events/logging/logfile_utils_test.cc
index 1c67312..e1e2ebd 100644
--- a/aos/events/logging/logfile_utils_test.cc
+++ b/aos/events/logging/logfile_utils_test.cc
@@ -230,14 +230,14 @@
BootTimestamp{.boot = 0, .time = e + chrono::milliseconds(1)},
.monotonic_remote_boot = 0xffffff,
.monotonic_timestamp_boot = 0xffffff,
- .data = SizePrefixedFlatbufferVector<MessageHeader>::Empty()};
+ .data = nullptr};
Message m2{.channel_index = 0,
.queue_index = BootQueueIndex{.boot = 0, .index = 0u},
.timestamp =
BootTimestamp{.boot = 0, .time = e + chrono::milliseconds(2)},
.monotonic_remote_boot = 0xffffff,
.monotonic_timestamp_boot = 0xffffff,
- .data = SizePrefixedFlatbufferVector<MessageHeader>::Empty()};
+ .data = nullptr};
EXPECT_LT(m1, m2);
EXPECT_GE(m2, m1);
@@ -847,22 +847,25 @@
EXPECT_EQ(output[0].timestamp.boot, 0u);
EXPECT_EQ(output[0].timestamp.time, e + chrono::milliseconds(101000));
- EXPECT_FALSE(output[0].data.message().has_monotonic_timestamp_time());
+ EXPECT_FALSE(output[0].data->has_monotonic_timestamp_time);
EXPECT_EQ(output[1].timestamp.boot, 0u);
EXPECT_EQ(output[1].timestamp.time, e + chrono::milliseconds(101001));
- EXPECT_TRUE(output[1].data.message().has_monotonic_timestamp_time());
- EXPECT_EQ(output[1].data.message().monotonic_timestamp_time(), 971);
+ EXPECT_TRUE(output[1].data->has_monotonic_timestamp_time);
+ EXPECT_EQ(output[1].data->monotonic_timestamp_time,
+ monotonic_clock::time_point(std::chrono::nanoseconds(971)));
EXPECT_EQ(output[2].timestamp.boot, 0u);
EXPECT_EQ(output[2].timestamp.time, e + chrono::milliseconds(101002));
- EXPECT_TRUE(output[2].data.message().has_monotonic_timestamp_time());
- EXPECT_EQ(output[2].data.message().monotonic_timestamp_time(), 972);
+ EXPECT_TRUE(output[2].data->has_monotonic_timestamp_time);
+ EXPECT_EQ(output[2].data->monotonic_timestamp_time,
+ monotonic_clock::time_point(std::chrono::nanoseconds(972)));
EXPECT_EQ(output[3].timestamp.boot, 0u);
EXPECT_EQ(output[3].timestamp.time, e + chrono::milliseconds(101003));
- EXPECT_TRUE(output[3].data.message().has_monotonic_timestamp_time());
- EXPECT_EQ(output[3].data.message().monotonic_timestamp_time(), 973);
+ EXPECT_TRUE(output[3].data->has_monotonic_timestamp_time);
+ EXPECT_EQ(output[3].data->monotonic_timestamp_time,
+ monotonic_clock::time_point(std::chrono::nanoseconds(973)));
}
// Tests that we can match timestamps on delivered messages.
@@ -941,17 +944,17 @@
EXPECT_EQ(output0[0].monotonic_event_time.boot, 0u);
EXPECT_EQ(output0[0].monotonic_event_time.time,
e + chrono::milliseconds(1000));
- EXPECT_TRUE(output0[0].data.Verify());
+ EXPECT_TRUE(output0[0].data != nullptr);
EXPECT_EQ(output0[1].monotonic_event_time.boot, 0u);
EXPECT_EQ(output0[1].monotonic_event_time.time,
e + chrono::milliseconds(2000));
- EXPECT_TRUE(output0[1].data.Verify());
+ EXPECT_TRUE(output0[1].data != nullptr);
EXPECT_EQ(output0[2].monotonic_event_time.boot, 0u);
EXPECT_EQ(output0[2].monotonic_event_time.time,
e + chrono::milliseconds(3000));
- EXPECT_TRUE(output0[2].data.Verify());
+ EXPECT_TRUE(output0[2].data != nullptr);
}
{
@@ -993,17 +996,17 @@
EXPECT_EQ(output1[0].monotonic_event_time.boot, 0u);
EXPECT_EQ(output1[0].monotonic_event_time.time,
e + chrono::seconds(100) + chrono::milliseconds(1000));
- EXPECT_TRUE(output1[0].data.Verify());
+ EXPECT_TRUE(output1[0].data != nullptr);
EXPECT_EQ(output1[1].monotonic_event_time.boot, 0u);
EXPECT_EQ(output1[1].monotonic_event_time.time,
e + chrono::seconds(100) + chrono::milliseconds(2000));
- EXPECT_TRUE(output1[1].data.Verify());
+ EXPECT_TRUE(output1[1].data != nullptr);
EXPECT_EQ(output1[2].monotonic_event_time.boot, 0u);
EXPECT_EQ(output1[2].monotonic_event_time.time,
e + chrono::seconds(100) + chrono::milliseconds(3000));
- EXPECT_TRUE(output1[2].data.Verify());
+ EXPECT_TRUE(output1[2].data != nullptr);
}
}
@@ -1072,7 +1075,7 @@
EXPECT_EQ(output0[0].monotonic_timestamp_time.boot, 0u);
EXPECT_EQ(output0[0].monotonic_timestamp_time.time,
monotonic_clock::min_time);
- EXPECT_TRUE(output0[0].data.Verify());
+ EXPECT_TRUE(output0[0].data != nullptr);
EXPECT_EQ(output0[1].monotonic_event_time.boot, 0u);
EXPECT_EQ(output0[1].monotonic_event_time.time,
@@ -1080,7 +1083,7 @@
EXPECT_EQ(output0[1].monotonic_timestamp_time.boot, 0u);
EXPECT_EQ(output0[1].monotonic_timestamp_time.time,
monotonic_clock::min_time);
- EXPECT_TRUE(output0[1].data.Verify());
+ EXPECT_TRUE(output0[1].data != nullptr);
EXPECT_EQ(output0[2].monotonic_event_time.boot, 0u);
EXPECT_EQ(output0[2].monotonic_event_time.time,
@@ -1088,7 +1091,7 @@
EXPECT_EQ(output0[2].monotonic_timestamp_time.boot, 0u);
EXPECT_EQ(output0[2].monotonic_timestamp_time.time,
monotonic_clock::min_time);
- EXPECT_TRUE(output0[2].data.Verify());
+ EXPECT_TRUE(output0[2].data != nullptr);
}
{
@@ -1109,7 +1112,7 @@
EXPECT_EQ(output1[0].monotonic_timestamp_time.boot, 0u);
EXPECT_EQ(output1[0].monotonic_timestamp_time.time,
e + chrono::nanoseconds(971));
- EXPECT_TRUE(output1[0].data.Verify());
+ EXPECT_TRUE(output1[0].data != nullptr);
EXPECT_EQ(output1[1].monotonic_event_time.boot, 0u);
EXPECT_EQ(output1[1].monotonic_event_time.time,
@@ -1117,7 +1120,7 @@
EXPECT_EQ(output1[1].monotonic_timestamp_time.boot, 0u);
EXPECT_EQ(output1[1].monotonic_timestamp_time.time,
e + chrono::nanoseconds(5458));
- EXPECT_TRUE(output1[1].data.Verify());
+ EXPECT_TRUE(output1[1].data != nullptr);
EXPECT_EQ(output1[2].monotonic_event_time.boot, 0u);
EXPECT_EQ(output1[2].monotonic_event_time.time,
@@ -1125,7 +1128,7 @@
EXPECT_EQ(output1[2].monotonic_timestamp_time.boot, 0u);
EXPECT_EQ(output1[2].monotonic_timestamp_time.time,
monotonic_clock::min_time);
- EXPECT_TRUE(output1[2].data.Verify());
+ EXPECT_TRUE(output1[2].data != nullptr);
}
EXPECT_EQ(mapper0_count, 3u);
@@ -1200,17 +1203,17 @@
EXPECT_EQ(output1[0].monotonic_event_time.boot, 0u);
EXPECT_EQ(output1[0].monotonic_event_time.time,
e + chrono::seconds(100) + chrono::milliseconds(1000));
- EXPECT_TRUE(output1[0].data.Verify());
+ EXPECT_TRUE(output1[0].data != nullptr);
EXPECT_EQ(output1[1].monotonic_event_time.boot, 0u);
EXPECT_EQ(output1[1].monotonic_event_time.time,
e + chrono::seconds(100) + chrono::milliseconds(2000));
- EXPECT_TRUE(output1[1].data.Verify());
+ EXPECT_TRUE(output1[1].data != nullptr);
EXPECT_EQ(output1[2].monotonic_event_time.boot, 0u);
EXPECT_EQ(output1[2].monotonic_event_time.time,
e + chrono::seconds(100) + chrono::milliseconds(3000));
- EXPECT_TRUE(output1[2].data.Verify());
+ EXPECT_TRUE(output1[2].data != nullptr);
}
{
@@ -1236,17 +1239,17 @@
EXPECT_EQ(output0[0].monotonic_event_time.boot, 0u);
EXPECT_EQ(output0[0].monotonic_event_time.time,
e + chrono::milliseconds(1000));
- EXPECT_TRUE(output0[0].data.Verify());
+ EXPECT_TRUE(output0[0].data != nullptr);
EXPECT_EQ(output0[1].monotonic_event_time.boot, 0u);
EXPECT_EQ(output0[1].monotonic_event_time.time,
e + chrono::milliseconds(2000));
- EXPECT_TRUE(output0[1].data.Verify());
+ EXPECT_TRUE(output0[1].data != nullptr);
EXPECT_EQ(output0[2].monotonic_event_time.boot, 0u);
EXPECT_EQ(output0[2].monotonic_event_time.time,
e + chrono::milliseconds(3000));
- EXPECT_TRUE(output0[2].data.Verify());
+ EXPECT_TRUE(output0[2].data != nullptr);
}
EXPECT_EQ(mapper0_count, 3u);
@@ -1319,17 +1322,17 @@
EXPECT_EQ(output1[0].monotonic_event_time.boot, 0u);
EXPECT_EQ(output1[0].monotonic_event_time.time,
e + chrono::seconds(100) + chrono::milliseconds(1000));
- EXPECT_FALSE(output1[0].data.Verify());
+ EXPECT_FALSE(output1[0].data != nullptr);
EXPECT_EQ(output1[1].monotonic_event_time.boot, 0u);
EXPECT_EQ(output1[1].monotonic_event_time.time,
e + chrono::seconds(100) + chrono::milliseconds(2000));
- EXPECT_TRUE(output1[1].data.Verify());
+ EXPECT_TRUE(output1[1].data != nullptr);
EXPECT_EQ(output1[2].monotonic_event_time.boot, 0u);
EXPECT_EQ(output1[2].monotonic_event_time.time,
e + chrono::seconds(100) + chrono::milliseconds(3000));
- EXPECT_TRUE(output1[2].data.Verify());
+ EXPECT_TRUE(output1[2].data != nullptr);
}
EXPECT_EQ(mapper0_count, 0u);
@@ -1402,17 +1405,17 @@
EXPECT_EQ(output1[0].monotonic_event_time.boot, 0u);
EXPECT_EQ(output1[0].monotonic_event_time.time,
e + chrono::seconds(100) + chrono::milliseconds(1000));
- EXPECT_TRUE(output1[0].data.Verify());
+ EXPECT_TRUE(output1[0].data != nullptr);
EXPECT_EQ(output1[1].monotonic_event_time.boot, 0u);
EXPECT_EQ(output1[1].monotonic_event_time.time,
e + chrono::seconds(100) + chrono::milliseconds(2000));
- EXPECT_TRUE(output1[1].data.Verify());
+ EXPECT_TRUE(output1[1].data != nullptr);
EXPECT_EQ(output1[2].monotonic_event_time.boot, 0u);
EXPECT_EQ(output1[2].monotonic_event_time.time,
e + chrono::seconds(100) + chrono::milliseconds(3000));
- EXPECT_FALSE(output1[2].data.Verify());
+ EXPECT_FALSE(output1[2].data != nullptr);
}
EXPECT_EQ(mapper0_count, 0u);
@@ -1477,12 +1480,12 @@
EXPECT_EQ(output1[0].monotonic_event_time.boot, 0u);
EXPECT_EQ(output1[0].monotonic_event_time.time,
e + chrono::seconds(100) + chrono::milliseconds(1000));
- EXPECT_TRUE(output1[0].data.Verify());
+ EXPECT_TRUE(output1[0].data != nullptr);
EXPECT_EQ(output1[1].monotonic_event_time.boot, 0u);
EXPECT_EQ(output1[1].monotonic_event_time.time,
e + chrono::seconds(100) + chrono::milliseconds(3000));
- EXPECT_TRUE(output1[1].data.Verify());
+ EXPECT_TRUE(output1[1].data != nullptr);
}
EXPECT_EQ(mapper0_count, 0u);
@@ -1550,22 +1553,22 @@
EXPECT_EQ(output1[0].monotonic_event_time.boot, 0u);
EXPECT_EQ(output1[0].monotonic_event_time.time,
e + chrono::seconds(100) + chrono::milliseconds(1000));
- EXPECT_TRUE(output1[0].data.Verify());
+ EXPECT_TRUE(output1[0].data != nullptr);
EXPECT_EQ(output1[1].monotonic_event_time.boot, 0u);
EXPECT_EQ(output1[1].monotonic_event_time.time,
e + chrono::seconds(100) + chrono::milliseconds(2000));
- EXPECT_TRUE(output1[1].data.Verify());
+ EXPECT_TRUE(output1[1].data != nullptr);
EXPECT_EQ(output1[2].monotonic_event_time.boot, 0u);
EXPECT_EQ(output1[2].monotonic_event_time.time,
e + chrono::seconds(100) + chrono::milliseconds(2000));
- EXPECT_TRUE(output1[2].data.Verify());
+ EXPECT_TRUE(output1[2].data != nullptr);
EXPECT_EQ(output1[3].monotonic_event_time.boot, 0u);
EXPECT_EQ(output1[3].monotonic_event_time.time,
e + chrono::seconds(100) + chrono::milliseconds(3000));
- EXPECT_TRUE(output1[3].data.Verify());
+ EXPECT_TRUE(output1[3].data != nullptr);
}
EXPECT_EQ(mapper0_count, 0u);
@@ -1650,17 +1653,17 @@
EXPECT_EQ(output1[0].monotonic_event_time.boot, 0u);
EXPECT_EQ(output1[0].monotonic_event_time.time,
e + chrono::seconds(100) + chrono::milliseconds(1000));
- EXPECT_FALSE(output1[0].data.Verify());
+ EXPECT_FALSE(output1[0].data != nullptr);
EXPECT_EQ(output1[1].monotonic_event_time.boot, 0u);
EXPECT_EQ(output1[1].monotonic_event_time.time,
e + chrono::seconds(100) + chrono::milliseconds(2000));
- EXPECT_FALSE(output1[1].data.Verify());
+ EXPECT_FALSE(output1[1].data != nullptr);
EXPECT_EQ(output1[2].monotonic_event_time.boot, 0u);
EXPECT_EQ(output1[2].monotonic_event_time.time,
e + chrono::seconds(100) + chrono::milliseconds(3000));
- EXPECT_FALSE(output1[2].data.Verify());
+ EXPECT_FALSE(output1[2].data != nullptr);
}
EXPECT_EQ(mapper1_count, 3u);
}
@@ -1754,22 +1757,22 @@
EXPECT_EQ(output0[0].monotonic_event_time.boot, 0u);
EXPECT_EQ(output0[0].monotonic_event_time.time,
e + chrono::milliseconds(1000));
- EXPECT_TRUE(output0[0].data.Verify());
+ EXPECT_TRUE(output0[0].data != nullptr);
EXPECT_EQ(output0[1].monotonic_event_time.boot, 0u);
EXPECT_EQ(output0[1].monotonic_event_time.time,
e + chrono::milliseconds(1000));
- EXPECT_TRUE(output0[1].data.Verify());
+ EXPECT_TRUE(output0[1].data != nullptr);
EXPECT_EQ(output0[2].monotonic_event_time.boot, 0u);
EXPECT_EQ(output0[2].monotonic_event_time.time,
e + chrono::milliseconds(2000));
- EXPECT_TRUE(output0[2].data.Verify());
+ EXPECT_TRUE(output0[2].data != nullptr);
EXPECT_EQ(output0[3].monotonic_event_time.boot, 0u);
EXPECT_EQ(output0[3].monotonic_event_time.time,
e + chrono::milliseconds(3000));
- EXPECT_TRUE(output0[3].data.Verify());
+ EXPECT_TRUE(output0[3].data != nullptr);
}
{
@@ -1827,22 +1830,22 @@
EXPECT_EQ(output1[0].monotonic_event_time.boot, 0u);
EXPECT_EQ(output1[0].monotonic_event_time.time,
e + chrono::seconds(100) + chrono::milliseconds(1000));
- EXPECT_TRUE(output1[0].data.Verify());
+ EXPECT_TRUE(output1[0].data != nullptr);
EXPECT_EQ(output1[1].monotonic_event_time.boot, 0u);
EXPECT_EQ(output1[1].monotonic_event_time.time,
e + chrono::seconds(100) + chrono::milliseconds(1000));
- EXPECT_TRUE(output1[1].data.Verify());
+ EXPECT_TRUE(output1[1].data != nullptr);
EXPECT_EQ(output1[2].monotonic_event_time.boot, 0u);
EXPECT_EQ(output1[2].monotonic_event_time.time,
e + chrono::seconds(100) + chrono::milliseconds(2000));
- EXPECT_TRUE(output1[2].data.Verify());
+ EXPECT_TRUE(output1[2].data != nullptr);
EXPECT_EQ(output1[3].monotonic_event_time.boot, 0u);
EXPECT_EQ(output1[3].monotonic_event_time.time,
e + chrono::seconds(100) + chrono::milliseconds(3000));
- EXPECT_TRUE(output1[3].data.Verify());
+ EXPECT_TRUE(output1[3].data != nullptr);
}
}
@@ -2194,7 +2197,7 @@
(BootQueueIndex{.boot = 0u, .index = 0u}));
EXPECT_EQ(output0[0].monotonic_remote_time, BootTimestamp::min_time());
EXPECT_EQ(output0[0].monotonic_timestamp_time, BootTimestamp::min_time());
- EXPECT_TRUE(output0[0].data.Verify());
+ EXPECT_TRUE(output0[0].data != nullptr);
EXPECT_EQ(output0[1].monotonic_event_time.boot, 0u);
EXPECT_EQ(output0[1].monotonic_event_time.time,
@@ -2203,7 +2206,7 @@
(BootQueueIndex{.boot = 0u, .index = 1u}));
EXPECT_EQ(output0[1].monotonic_remote_time, BootTimestamp::min_time());
EXPECT_EQ(output0[1].monotonic_timestamp_time, BootTimestamp::min_time());
- EXPECT_TRUE(output0[1].data.Verify());
+ EXPECT_TRUE(output0[1].data != nullptr);
EXPECT_EQ(output0[2].monotonic_event_time.boot, 0u);
EXPECT_EQ(output0[2].monotonic_event_time.time,
@@ -2212,7 +2215,7 @@
(BootQueueIndex{.boot = 0u, .index = 2u}));
EXPECT_EQ(output0[2].monotonic_remote_time, BootTimestamp::min_time());
EXPECT_EQ(output0[2].monotonic_timestamp_time, BootTimestamp::min_time());
- EXPECT_TRUE(output0[2].data.Verify());
+ EXPECT_TRUE(output0[2].data != nullptr);
}
{
@@ -2267,7 +2270,7 @@
EXPECT_EQ(output1[0].monotonic_timestamp_time.boot, 0u);
EXPECT_EQ(output1[0].monotonic_timestamp_time.time,
e + chrono::milliseconds(1001));
- EXPECT_TRUE(output1[0].data.Verify());
+ EXPECT_TRUE(output1[0].data != nullptr);
EXPECT_EQ(output1[1].monotonic_event_time.boot, 1u);
EXPECT_EQ(output1[1].monotonic_event_time.time,
@@ -2280,7 +2283,7 @@
EXPECT_EQ(output1[1].monotonic_timestamp_time.boot, 0u);
EXPECT_EQ(output1[1].monotonic_timestamp_time.time,
e + chrono::milliseconds(2001));
- EXPECT_TRUE(output1[1].data.Verify());
+ EXPECT_TRUE(output1[1].data != nullptr);
EXPECT_EQ(output1[2].monotonic_event_time.boot, 1u);
EXPECT_EQ(output1[2].monotonic_event_time.time,
@@ -2293,7 +2296,7 @@
EXPECT_EQ(output1[2].monotonic_timestamp_time.boot, 0u);
EXPECT_EQ(output1[2].monotonic_timestamp_time.time,
e + chrono::milliseconds(2001));
- EXPECT_TRUE(output1[2].data.Verify());
+ EXPECT_TRUE(output1[2].data != nullptr);
EXPECT_EQ(output1[3].monotonic_event_time.boot, 1u);
EXPECT_EQ(output1[3].monotonic_event_time.time,
@@ -2306,7 +2309,7 @@
EXPECT_EQ(output1[3].monotonic_timestamp_time.boot, 0u);
EXPECT_EQ(output1[3].monotonic_timestamp_time.time,
e + chrono::milliseconds(3001));
- EXPECT_TRUE(output1[3].data.Verify());
+ EXPECT_TRUE(output1[3].data != nullptr);
LOG(INFO) << output1[0];
LOG(INFO) << output1[1];
@@ -2414,7 +2417,7 @@
EXPECT_EQ(output0[0].monotonic_timestamp_time.boot, 0u);
EXPECT_EQ(output0[0].monotonic_timestamp_time.time,
e + chrono::seconds(100) + chrono::milliseconds(1001));
- EXPECT_TRUE(output0[0].data.Verify());
+ EXPECT_TRUE(output0[0].data != nullptr);
EXPECT_EQ(output0[1].monotonic_event_time.boot, 0u);
EXPECT_EQ(output0[1].monotonic_event_time.time,
@@ -2425,7 +2428,7 @@
EXPECT_EQ(output0[1].monotonic_timestamp_time.boot, 0u);
EXPECT_EQ(output0[1].monotonic_timestamp_time.time,
e + chrono::seconds(20) + chrono::milliseconds(2001));
- EXPECT_TRUE(output0[1].data.Verify());
+ EXPECT_TRUE(output0[1].data != nullptr);
EXPECT_EQ(output0[2].monotonic_event_time.boot, 0u);
EXPECT_EQ(output0[2].monotonic_event_time.time,
@@ -2436,7 +2439,7 @@
EXPECT_EQ(output0[2].monotonic_timestamp_time.boot, 0u);
EXPECT_EQ(output0[2].monotonic_timestamp_time.time,
e + chrono::seconds(20) + chrono::milliseconds(3001));
- EXPECT_TRUE(output0[2].data.Verify());
+ EXPECT_TRUE(output0[2].data != nullptr);
}
{
@@ -2480,21 +2483,21 @@
e + chrono::seconds(100) + chrono::milliseconds(1000));
EXPECT_EQ(output1[0].monotonic_remote_time, BootTimestamp::min_time());
EXPECT_EQ(output1[0].monotonic_timestamp_time, BootTimestamp::min_time());
- EXPECT_TRUE(output1[0].data.Verify());
+ EXPECT_TRUE(output1[0].data != nullptr);
EXPECT_EQ(output1[1].monotonic_event_time.boot, 1u);
EXPECT_EQ(output1[1].monotonic_event_time.time,
e + chrono::seconds(20) + chrono::milliseconds(2000));
EXPECT_EQ(output1[1].monotonic_remote_time, BootTimestamp::min_time());
EXPECT_EQ(output1[1].monotonic_timestamp_time, BootTimestamp::min_time());
- EXPECT_TRUE(output1[1].data.Verify());
+ EXPECT_TRUE(output1[1].data != nullptr);
EXPECT_EQ(output1[2].monotonic_event_time.boot, 1u);
EXPECT_EQ(output1[2].monotonic_event_time.time,
e + chrono::seconds(20) + chrono::milliseconds(3000));
EXPECT_EQ(output1[2].monotonic_remote_time, BootTimestamp::min_time());
EXPECT_EQ(output1[2].monotonic_timestamp_time, BootTimestamp::min_time());
- EXPECT_TRUE(output1[2].data.Verify());
+ EXPECT_TRUE(output1[2].data != nullptr);
LOG(INFO) << output1[0];
LOG(INFO) << output1[1];
diff --git a/aos/events/logging/logger_test.cc b/aos/events/logging/logger_test.cc
index 0be64bb..a923cf6 100644
--- a/aos/events/logging/logger_test.cc
+++ b/aos/events/logging/logger_test.cc
@@ -1,9 +1,8 @@
-#include "aos/events/logging/log_reader.h"
-
#include <sys/stat.h>
#include "absl/strings/str_format.h"
#include "aos/events/event_loop.h"
+#include "aos/events/logging/log_reader.h"
#include "aos/events/logging/log_writer.h"
#include "aos/events/message_counter.h"
#include "aos/events/ping_lib.h"
@@ -849,19 +848,18 @@
// type, count) for every message matching matcher()
std::vector<std::tuple<std::string, std::string, int>> CountChannelsMatching(
std::shared_ptr<const aos::Configuration> config, std::string_view filename,
- std::function<bool(const MessageHeader *)> matcher) {
+ std::function<bool(const UnpackedMessageHeader *)> matcher) {
MessageReader message_reader(filename);
std::vector<int> counts(config->channels()->size(), 0);
while (true) {
- std::optional<SizePrefixedFlatbufferVector<MessageHeader>> msg =
- message_reader.ReadMessage();
+ std::shared_ptr<UnpackedMessageHeader> msg = message_reader.ReadMessage();
if (!msg) {
break;
}
- if (matcher(&msg.value().message())) {
- counts[msg.value().message().channel_index()]++;
+ if (matcher(msg.get())) {
+ counts[msg->channel_index]++;
}
}
@@ -883,30 +881,32 @@
std::vector<std::tuple<std::string, std::string, int>> CountChannelsData(
std::shared_ptr<const aos::Configuration> config,
std::string_view filename) {
- return CountChannelsMatching(config, filename, [](const MessageHeader *msg) {
- if (msg->has_data()) {
- CHECK(!msg->has_monotonic_remote_time());
- CHECK(!msg->has_realtime_remote_time());
- CHECK(!msg->has_remote_queue_index());
- return true;
- }
- return false;
- });
+ return CountChannelsMatching(
+ config, filename, [](const UnpackedMessageHeader *msg) {
+ if (msg->span.data() != nullptr) {
+ CHECK(!msg->monotonic_remote_time.has_value());
+ CHECK(!msg->realtime_remote_time.has_value());
+ CHECK(!msg->remote_queue_index.has_value());
+ return true;
+ }
+ return false;
+ });
}
// Counts the number of messages (channel, count) for all timestamp messages.
std::vector<std::tuple<std::string, std::string, int>> CountChannelsTimestamp(
std::shared_ptr<const aos::Configuration> config,
std::string_view filename) {
- return CountChannelsMatching(config, filename, [](const MessageHeader *msg) {
- if (!msg->has_data()) {
- CHECK(msg->has_monotonic_remote_time());
- CHECK(msg->has_realtime_remote_time());
- CHECK(msg->has_remote_queue_index());
- return true;
- }
- return false;
- });
+ return CountChannelsMatching(
+ config, filename, [](const UnpackedMessageHeader *msg) {
+ if (msg->span.data() == nullptr) {
+ CHECK(msg->monotonic_remote_time.has_value());
+ CHECK(msg->realtime_remote_time.has_value());
+ CHECK(msg->remote_queue_index.has_value());
+ return true;
+ }
+ return false;
+ });
}
// Tests that we can write and read simple multi-node log files.
diff --git a/aos/events/simulated_event_loop.cc b/aos/events/simulated_event_loop.cc
index 3cd5c5b..9121a8f 100644
--- a/aos/events/simulated_event_loop.cc
+++ b/aos/events/simulated_event_loop.cc
@@ -39,38 +39,71 @@
const bool prior_;
};
+// Holds storage for a span object and the data referenced by that span for
+// compatibility with RawSender::SharedSpan users. If constructed with
+// MakeSharedSpan, span points to only the aligned segment of the entire data.
+struct AlignedOwningSpan {
+ AlignedOwningSpan(const AlignedOwningSpan &) = delete;
+ AlignedOwningSpan &operator=(const AlignedOwningSpan &) = delete;
+ absl::Span<const uint8_t> span;
+ char data[];
+};
+
+// Constructs a span which owns its data through a shared_ptr. The owning span
+// points to a const view of the data; also returns a temporary mutable span
+// which is only valid while the const shared span is kept alive.
+std::pair<RawSender::SharedSpan, absl::Span<uint8_t>> MakeSharedSpan(
+ size_t size) {
+ AlignedOwningSpan *const span = reinterpret_cast<AlignedOwningSpan *>(
+ malloc(sizeof(AlignedOwningSpan) + size + kChannelDataAlignment - 1));
+
+ absl::Span mutable_span(
+ reinterpret_cast<uint8_t *>(RoundChannelData(&span->data[0], size)),
+ size);
+ new (span) AlignedOwningSpan{.span = mutable_span};
+
+ return std::make_pair(
+ RawSender::SharedSpan(
+ std::shared_ptr<AlignedOwningSpan>(span,
+ [](AlignedOwningSpan *s) {
+ s->~AlignedOwningSpan();
+ free(s);
+ }),
+ &span->span),
+ mutable_span);
+}
+
// Container for both a message, and the context for it for simulation. This
// makes tracking the timestamps associated with the data easy.
struct SimulatedMessage final {
SimulatedMessage(const SimulatedMessage &) = delete;
SimulatedMessage &operator=(const SimulatedMessage &) = delete;
+ ~SimulatedMessage();
// Creates a SimulatedMessage with size bytes of storage.
// This is a shared_ptr so we don't have to implement refcounting or copying.
- static std::shared_ptr<SimulatedMessage> Make(SimulatedChannel *channel);
+ static std::shared_ptr<SimulatedMessage> Make(
+ SimulatedChannel *channel, const RawSender::SharedSpan data);
// Context for the data.
Context context;
SimulatedChannel *const channel = nullptr;
- // The data.
- char *data(size_t buffer_size) {
- return RoundChannelData(&actual_data[0], buffer_size);
- }
+ // Owning span to this message's data. Depending on the sender may either
+ // represent the data of just the flatbuffer, or max channel size.
+ RawSender::SharedSpan data;
- // Then the data, including padding on the end so we can align the buffer we
- // actually return from data().
- char actual_data[];
+ // Mutable view of above data. If empty, this message is not mutable.
+ absl::Span<uint8_t> mutable_data;
- private:
+ // Determines whether this message is mutable. Used for Send where the user
+ // fills out a message stored internally then gives us the size of data used.
+ bool is_mutable() const { return data->size() == mutable_data.size(); }
+
+ // Note: this should be private but make_shared requires it to be public. Use
+ // Make() above to construct.
SimulatedMessage(SimulatedChannel *channel_in);
- ~SimulatedMessage();
-
- static void DestroyAndFree(SimulatedMessage *p) {
- p->~SimulatedMessage();
- free(p);
- }
};
} // namespace
@@ -260,19 +293,17 @@
namespace {
std::shared_ptr<SimulatedMessage> SimulatedMessage::Make(
- SimulatedChannel *channel) {
+ SimulatedChannel *channel, RawSender::SharedSpan data) {
// The allocations in here are due to infrastructure and don't count in the no
// mallocs in RT code.
ScopedNotRealtime nrt;
- const size_t size = channel->max_size();
- SimulatedMessage *const message = reinterpret_cast<SimulatedMessage *>(
- malloc(sizeof(SimulatedMessage) + size + kChannelDataAlignment - 1));
- new (message) SimulatedMessage(channel);
- message->context.size = size;
- message->context.data = message->data(size);
- return std::shared_ptr<SimulatedMessage>(message,
- &SimulatedMessage::DestroyAndFree);
+ auto message = std::make_shared<SimulatedMessage>(channel);
+ message->context.size = data->size();
+ message->context.data = data->data();
+ message->data = std::move(data);
+
+ return message;
}
SimulatedMessage::SimulatedMessage(SimulatedChannel *channel_in)
@@ -292,9 +323,13 @@
void *data() override {
if (!message_) {
- message_ = SimulatedMessage::Make(simulated_channel_);
+ auto [span, mutable_span] =
+ MakeSharedSpan(simulated_channel_->max_size());
+ message_ = SimulatedMessage::Make(simulated_channel_, span);
+ message_->mutable_data = mutable_span;
}
- return message_->data(simulated_channel_->max_size());
+ CHECK(message_->is_mutable());
+ return message_->mutable_data.data();
}
size_t size() override { return simulated_channel_->max_size(); }
@@ -310,6 +345,12 @@
uint32_t remote_queue_index,
const UUID &source_boot_uuid) override;
+ bool DoSend(const SharedSpan data,
+ aos::monotonic_clock::time_point monotonic_remote_time,
+ aos::realtime_clock::time_point realtime_remote_time,
+ uint32_t remote_queue_index,
+ const UUID &source_boot_uuid) override;
+
int buffer_index() override {
// First, ensure message_ is allocated.
data();
@@ -869,8 +910,12 @@
uint32_t SimulatedChannel::Send(std::shared_ptr<SimulatedMessage> message) {
const uint32_t queue_index = next_queue_index_.index();
message->context.queue_index = queue_index;
- message->context.data = message->data(channel()->max_size()) +
- channel()->max_size() - message->context.size;
+
+ // Points to the actual data depending on the size set in context. Data may
+ // allocate more than the actual size of the message, so offset from the back
+ // of that to get the actual start of the data.
+ message->context.data =
+ message->data->data() + message->data->size() - message->context.size;
DCHECK(channel()->has_schema())
<< ": Missing schema for channel "
@@ -959,21 +1004,35 @@
<< ": Attempting to send too big a message on "
<< configuration::CleanedChannelToString(simulated_channel_->channel());
- // This is wasteful, but since flatbuffers fill from the back end of the
- // queue, we need it to be full sized.
- message_ = SimulatedMessage::Make(simulated_channel_);
+ // Allocates an aligned buffer in which to copy unaligned msg.
+ auto [span, mutable_span] = MakeSharedSpan(size);
+ message_ = SimulatedMessage::Make(simulated_channel_, span);
// Now fill in the message. size is already populated above, and
- // queue_index will be populated in simulated_channel_. Put this at the
- // back of the data segment.
- memcpy(message_->data(simulated_channel_->max_size()) +
- simulated_channel_->max_size() - size,
- msg, size);
+ // queue_index will be populated in simulated_channel_.
+ memcpy(mutable_span.data(), msg, size);
return DoSend(size, monotonic_remote_time, realtime_remote_time,
remote_queue_index, source_boot_uuid);
}
+bool SimulatedSender::DoSend(const RawSender::SharedSpan data,
+ monotonic_clock::time_point monotonic_remote_time,
+ realtime_clock::time_point realtime_remote_time,
+ uint32_t remote_queue_index,
+ const UUID &source_boot_uuid) {
+ CHECK_LE(data->size(), this->size())
+ << ": Attempting to send too big a message on "
+ << configuration::CleanedChannelToString(simulated_channel_->channel());
+
+ // Constructs a message sharing the already allocated and aligned message
+ // data.
+ message_ = SimulatedMessage::Make(simulated_channel_, data);
+
+ return DoSend(data->size(), monotonic_remote_time, realtime_remote_time,
+ remote_queue_index, source_boot_uuid);
+}
+
SimulatedTimerHandler::SimulatedTimerHandler(
EventScheduler *scheduler, SimulatedEventLoop *simulated_event_loop,
::std::function<void()> fn)
diff --git a/aos/flatbuffer_introspection.cc b/aos/flatbuffer_introspection.cc
index 5f6ca45..b7f3727 100644
--- a/aos/flatbuffer_introspection.cc
+++ b/aos/flatbuffer_introspection.cc
@@ -200,9 +200,9 @@
reflection::BaseType elem_type = type->element();
if (vector->size() > json_options.max_vector_size) {
- out->Append("[ ... ");
+ out->Append("[ \"... ");
out->AppendInt(vector->size());
- out->Append(" elements ... ]");
+ out->Append(" elements ...\" ]");
break;
}
diff --git a/aos/flatbuffer_introspection_test.cc b/aos/flatbuffer_introspection_test.cc
index fe8460f..e435709 100644
--- a/aos/flatbuffer_introspection_test.cc
+++ b/aos/flatbuffer_introspection_test.cc
@@ -362,7 +362,7 @@
std::string out =
FlatbufferToJson(schema_, builder.GetBufferPointer(),
{.multi_line = false, .max_vector_size = 100});
- EXPECT_EQ(out, "{\"vector_foo_int\": [ ... 101 elements ... ]}");
+ EXPECT_EQ(out, "{\"vector_foo_int\": [ \"... 101 elements ...\" ]}");
}
TEST_F(FlatbufferIntrospectionTest, MultilineTest) {
diff --git a/aos/json_to_flatbuffer.cc b/aos/json_to_flatbuffer.cc
index 11ea3a2..6d4a2f1 100644
--- a/aos/json_to_flatbuffer.cc
+++ b/aos/json_to_flatbuffer.cc
@@ -840,7 +840,7 @@
}
if (size > max_vector_size_) {
++skip_levels_;
- to_string_.s += "[ ... " + std::to_string(size) + " elements ... ]";
+ to_string_.s += "[ \"... " + std::to_string(size) + " elements ...\" ]";
return;
}
to_string_.StartVector(size);
diff --git a/aos/json_to_flatbuffer_test.cc b/aos/json_to_flatbuffer_test.cc
index e353b99..42457f6 100644
--- a/aos/json_to_flatbuffer_test.cc
+++ b/aos/json_to_flatbuffer_test.cc
@@ -270,9 +270,9 @@
EXPECT_EQ(json_short, back_json_short_typetable);
EXPECT_EQ(json_short, back_json_short_reflection);
- EXPECT_EQ("{ \"vector_foo_int\": [ ... 101 elements ... ] }",
+ EXPECT_EQ("{ \"vector_foo_int\": [ \"... 101 elements ...\" ] }",
back_json_long_typetable);
- EXPECT_EQ("{ \"vector_foo_int\": [ ... 101 elements ... ] }",
+ EXPECT_EQ("{ \"vector_foo_int\": [ \"... 101 elements ...\" ] }",
back_json_long_reflection);
}
diff --git a/aos/starter/starter.sh b/aos/starter/starter.sh
index 68d567a..2c8494e 100755
--- a/aos/starter/starter.sh
+++ b/aos/starter/starter.sh
@@ -17,5 +17,5 @@
cd "${ROBOT_CODE}"
export PATH="${PATH}:${ROBOT_CODE}"
while true; do
- starterd.stripped 2>&1
+ starterd 2>&1
done
diff --git a/aos/starter/starter_cmd.cc b/aos/starter/starter_cmd.cc
index 877b0af..d306306 100644
--- a/aos/starter/starter_cmd.cc
+++ b/aos/starter/starter_cmd.cc
@@ -14,6 +14,12 @@
DEFINE_string(config, "./config.json", "File path of aos configuration");
+DEFINE_bool(_bash_autocomplete, false,
+ "Internal use: Outputs commands or applications for use with "
+ "autocomplete script.");
+DEFINE_string(_bash_autocomplete_word, "",
+ "Internal use: Current word being autocompleted");
+
namespace {
namespace chrono = std::chrono;
@@ -71,19 +77,19 @@
// Prints the status for all applications.
void GetAllStarterStatus(const aos::Configuration *config) {
- // Print status for all processes.
- const auto optional_status = aos::starter::GetStarterStatus(config);
- if (optional_status) {
- auto status = *optional_status;
- const auto time = aos::monotonic_clock::now();
- PrintKey();
- for (const aos::starter::ApplicationStatus *app_status :
- *status.message().statuses()) {
- PrintApplicationStatus(app_status, time);
- }
- } else {
- LOG(WARNING) << "No status found";
+ // Print status for all processes.
+ const auto optional_status = aos::starter::GetStarterStatus(config);
+ if (optional_status) {
+ auto status = *optional_status;
+ const auto time = aos::monotonic_clock::now();
+ PrintKey();
+ for (const aos::starter::ApplicationStatus *app_status :
+ *status.message().statuses()) {
+ PrintApplicationStatus(app_status, time);
}
+ } else {
+ LOG(WARNING) << "No status found";
+ }
}
// Handles the "status" command. Returns true if the help message should be
@@ -251,6 +257,34 @@
return false;
}
+void Autocomplete(int argc, char **argv, const aos::Configuration *config) {
+ const std::string_view command = (argc >= 2 ? argv[1] : "");
+ const std::string_view app_name = (argc >= 3 ? argv[2] : "");
+
+ std::cout << "COMPREPLY=(";
+ if (FLAGS__bash_autocomplete_word == command) {
+ // Autocomplete the starter command
+ for (const auto &entry : kCommands) {
+ if (std::get<0>(entry).find(command) == 0) {
+ std::cout << '\'' << std::get<0>(entry) << "' ";
+ }
+ }
+ } else {
+ // Autocomplete the app name
+ for (const auto *app : *config->applications()) {
+ if (app->has_name() && app->name()->string_view().find(app_name) == 0) {
+ std::cout << '\'' << app->name()->string_view() << "' ";
+ }
+ }
+
+ // Autocomplete with "all"
+ if (std::string_view("all").find(app_name) == 0) {
+ std::cout << "'all'";
+ }
+ }
+ std::cout << ')';
+}
+
} // namespace
int main(int argc, char **argv) {
@@ -259,6 +293,11 @@
aos::FlatbufferDetachedBuffer<aos::Configuration> config =
aos::configuration::ReadConfig(FLAGS_config);
+ if (FLAGS__bash_autocomplete) {
+ Autocomplete(argc, argv, &config.message());
+ return 0;
+ }
+
bool parsing_failed = false;
if (argc < 2) {
diff --git a/frc971/control_loops/drivetrain/hybrid_ekf.h b/frc971/control_loops/drivetrain/hybrid_ekf.h
index ecdabb8..2a972b6 100644
--- a/frc971/control_loops/drivetrain/hybrid_ekf.h
+++ b/frc971/control_loops/drivetrain/hybrid_ekf.h
@@ -137,7 +137,7 @@
};
static constexpr int kNInputs = 4;
// Number of previous samples to save.
- static constexpr int kSaveSamples = 80;
+ static constexpr int kSaveSamples = 200;
// Whether we should completely rerun the entire stored history of
// kSaveSamples on every correction. Enabling this will increase overall CPU
// usage substantially; however, leaving it disabled makes it so that we are
diff --git a/frc971/downloader/downloader.bzl b/frc971/downloader/downloader.bzl
index e1b535e..9eb7e32 100644
--- a/frc971/downloader/downloader.bzl
+++ b/frc971/downloader/downloader.bzl
@@ -1,5 +1,12 @@
def _aos_downloader_impl(ctx):
all_files = ctx.files.srcs + ctx.files.start_srcs + [ctx.outputs._startlist]
+ target_files = []
+
+ # downloader looks for : in the inputs and uses the part after the : as
+ # the directory to copy to.
+ for d in ctx.attr.dirs:
+ target_files += [src.short_path + ":" + d.downloader_dir for src in d.downloader_srcs]
+
ctx.actions.write(
output = ctx.outputs.executable,
is_executable = True,
@@ -7,16 +14,12 @@
"#!/bin/bash",
"set -e",
'cd "${BASH_SOURCE[0]}.runfiles/%s"' % ctx.workspace_name,
- ] + ['%s --dir %s --target "$@" --type %s %s' % (
- ctx.executable._downloader.short_path,
- d.downloader_dir,
- ctx.attr.target_type,
- " ".join([src.short_path for src in d.downloader_srcs]),
- ) for d in ctx.attr.dirs] + [
- 'exec %s --target "$@" --type %s %s' % (
+ ] + [
+ 'exec %s --target "$@" --type %s %s %s' % (
ctx.executable._downloader.short_path,
ctx.attr.target_type,
" ".join([src.short_path for src in all_files]),
+ " ".join(target_files),
),
]),
)
diff --git a/frc971/downloader/downloader.py b/frc971/downloader/downloader.py
index c228a97..dc14df1 100644
--- a/frc971/downloader/downloader.py
+++ b/frc971/downloader/downloader.py
@@ -6,9 +6,12 @@
import argparse
import sys
+from tempfile import TemporaryDirectory
import subprocess
import re
+import stat
import os
+import shutil
def install(ssh_target, pkg, ssh_path, scp_path):
@@ -28,39 +31,30 @@
def main(argv):
parser = argparse.ArgumentParser()
- parser.add_argument("--target",
- type=str,
- default="roborio-971-frc.local",
- help="Target to deploy code to.")
- parser.add_argument("--type",
- type=str,
- choices=["roborio", "pi"],
- required=True,
- help="Target type for deployment")
parser.add_argument(
- "--dir",
+ "--target",
type=str,
- help="Directory within robot_code to copy the files to.")
- parser.add_argument("srcs",
- type=str,
- nargs='+',
- help="List of files to copy over")
+ default="roborio-971-frc.local",
+ help="Target to deploy code to.")
+ parser.add_argument(
+ "--type",
+ type=str,
+ choices=["roborio", "pi"],
+ required=True,
+ help="Target type for deployment")
+ parser.add_argument(
+ "srcs", type=str, nargs='+', help="List of files to copy over")
args = parser.parse_args(argv[1:])
- relative_dir = ""
- recursive = False
-
srcs = args.srcs
- if args.dir is not None:
- relative_dir = args.dir
- recursive = True
destination = args.target
result = re.match("(?:([^:@]+)@)?([^:@]+)(?::([^:@]+))?", destination)
if not result:
- print("Not sure how to parse destination \"%s\"!" % destination,
- file=sys.stderr)
+ print(
+ "Not sure how to parse destination \"%s\"!" % destination,
+ file=sys.stderr)
return 1
user = None
if result.group(1):
@@ -82,33 +76,74 @@
ssh_path = "external/ssh/ssh"
scp_path = "external/ssh/scp"
- subprocess.check_call([ssh_path, ssh_target, "mkdir", "-p", target_dir])
+ # Since rsync is pretty fixed in what it can do, build up a temporary
+ # directory with the exact contents we want the target to have. This
+ # is faster than multiple SSH connections.
+ with TemporaryDirectory() as temp_dir:
+ pwd = os.getcwd()
+ # Bazel gives us the same file twice, so dedup here rather than
+ # in starlark
+ copied = set()
+ for s in srcs:
+ if ":" in s:
+ folder = os.path.join(temp_dir, s[s.find(":") + 1:])
+ os.makedirs(folder, exist_ok=True)
+ s = os.path.join(pwd, s[:s.find(":")])
+ destination = os.path.join(folder, os.path.basename(s))
+ else:
+ s = os.path.join(pwd, s)
+ destination = os.path.join(temp_dir, os.path.basename(s))
- rsync_cmd = ([
- "external/rsync/usr/bin/rsync", "-e", ssh_path, "-c", "-v", "-z",
- "--perms", "--copy-links"
- ] + srcs + ["%s:%s/%s" % (ssh_target, target_dir, relative_dir)])
- try:
- subprocess.check_call(rsync_cmd)
- except subprocess.CalledProcessError as e:
- if e.returncode == 127 or e.returncode == 12:
- print("Unconfigured roboRIO, installing rsync.")
- install(ssh_target, "libattr1_2.4.47-r0.36_cortexa9-vfpv3.ipk",
- ssh_path, scp_path)
- install(ssh_target, "libacl1_2.2.52-r0.36_cortexa9-vfpv3.ipk",
- ssh_path, scp_path)
- install(ssh_target, "rsync_3.1.0-r0.7_cortexa9-vfpv3.ipk",
- ssh_path, scp_path)
- subprocess.check_call(rsync_cmd)
+ if s in copied:
+ continue
+ copied.add(s)
+ if s.endswith(".stripped"):
+ destination = destination[:destination.find(".stripped")]
+ shutil.copy2(s, destination)
+ # Make sure the folder that gets created on the roboRIO has open
+ # permissions or the executables won't be visible to init.
+ os.chmod(temp_dir, 0o775)
+ # Starter needs to be SUID so we transition from lvuser to admin.
+ os.chmod(os.path.join(temp_dir, "starterd"), 0o775 | stat.S_ISUID)
+
+ rsync_cmd = ([
+ "external/rsync/usr/bin/rsync",
+ "-e",
+ ssh_path,
+ "-c",
+ "-r",
+ "-v",
+ "--perms",
+ "-l",
+ temp_dir + "/",
+ ])
+
+ # If there is only 1 file to transfer, we would overwrite the destination
+ # folder. In that case, specify the full path to the target.
+ if len(srcs) == 1:
+ rsync_cmd += ["%s:%s/%s" % (ssh_target, target_dir, srcs[0])]
else:
- raise e
+ rsync_cmd += ["%s:%s" % (ssh_target, target_dir)]
- if not recursive:
- subprocess.check_call((ssh_path, ssh_target, "&&".join([
- "chmod u+s %s/starterd.stripped" % target_dir,
- "echo \'Done moving new executables into place\'",
- "bash -c \'sync && sync && sync\'",
- ])))
+ try:
+ subprocess.check_call(rsync_cmd)
+ except subprocess.CalledProcessError as e:
+ if e.returncode == 127 or e.returncode == 12:
+ print("Unconfigured roboRIO, installing rsync.")
+ install(ssh_target, "libattr1_2.4.47-r0.36_cortexa9-vfpv3.ipk",
+ ssh_path, scp_path)
+ install(ssh_target, "libacl1_2.2.52-r0.36_cortexa9-vfpv3.ipk",
+ ssh_path, scp_path)
+ install(ssh_target, "rsync_3.1.0-r0.7_cortexa9-vfpv3.ipk",
+ ssh_path, scp_path)
+ subprocess.check_call(rsync_cmd)
+ elif e.returncode == 11:
+ # Directory wasn't created, make it and try again. This keeps the happy path fast.
+ subprocess.check_call(
+ [ssh_path, ssh_target, "mkdir", "-p", target_dir])
+ subprocess.check_call(rsync_cmd)
+ else:
+ raise e
if __name__ == "__main__":
diff --git a/frc971/input/action_joystick_input.h b/frc971/input/action_joystick_input.h
index cdd0aa6..3284b15 100644
--- a/frc971/input/action_joystick_input.h
+++ b/frc971/input/action_joystick_input.h
@@ -65,6 +65,11 @@
return drivetrain_input_reader_->robot_velocity();
}
+ // Returns the current drivetrain status.
+ const control_loops::drivetrain::Status *drivetrain_status() const {
+ return drivetrain_input_reader_->drivetrain_status();
+ }
+
// Returns the drivetrain config.
const ::frc971::control_loops::drivetrain::DrivetrainConfig<double>
dt_config() const {
diff --git a/frc971/input/drivetrain_input.h b/frc971/input/drivetrain_input.h
index 3419ea7..a33d449 100644
--- a/frc971/input/drivetrain_input.h
+++ b/frc971/input/drivetrain_input.h
@@ -94,6 +94,11 @@
// Returns the current robot velocity in m/s.
double robot_velocity() const { return robot_velocity_; }
+ // Returns the current drivetrain status.
+ const control_loops::drivetrain::Status *drivetrain_status() const {
+ return drivetrain_status_fetcher_.get();
+ }
+
void set_vision_align_fn(
::std::function<bool(const ::frc971::input::driver_station::Data &data)>
vision_align_fn) {
diff --git a/third_party/allwpilib/hal/src/main/native/athena/Power.cpp b/third_party/allwpilib/hal/src/main/native/athena/Power.cpp
index 2cf4d33..a13ebb6 100644
--- a/third_party/allwpilib/hal/src/main/native/athena/Power.cpp
+++ b/third_party/allwpilib/hal/src/main/native/athena/Power.cpp
@@ -29,7 +29,12 @@
namespace hal {
namespace init {
-void InitializePower() {}
+void InitializePower() {
+ if (power == nullptr) {
+ int32_t status = 0;
+ power.reset(tPower::create(&status));
+ }
+}
} // namespace init
} // namespace hal
diff --git a/y2019/y2019.json b/y2019/y2019.json
index 719925c..5b34951 100644
--- a/y2019/y2019.json
+++ b/y2019/y2019.json
@@ -44,35 +44,35 @@
"applications": [
{
"name": "drivetrain",
- "executable_name": "drivetrain.stripped"
+ "executable_name": "drivetrain"
},
{
"name": "trajectory_generator",
- "executable_name": "trajectory_generator.stripped"
+ "executable_name": "trajectory_generator"
},
{
"name": "superstructure",
- "executable_name": "superstructure.stripped"
+ "executable_name": "superstructure"
},
{
"name": "server",
- "executable_name": "server.stripped"
+ "executable_name": "server"
},
{
"name": "logger_main",
- "executable_name": "logger_main.stripped"
+ "executable_name": "logger_main"
},
{
"name": "joystick_reader",
- "executable_name": "joystick_reader.stripped"
+ "executable_name": "joystick_reader"
},
{
"name": "autonomous_action",
- "executable_name": "autonomous_action.stripped"
+ "executable_name": "autonomous_action"
},
{
"name": "wpilib_interface",
- "executable_name": "wpilib_interface.stripped"
+ "executable_name": "wpilib_interface"
}
],
"imports": [
diff --git a/y2020/BUILD b/y2020/BUILD
index ef4e0f6..195e2f4 100644
--- a/y2020/BUILD
+++ b/y2020/BUILD
@@ -140,6 +140,7 @@
"//frc971/input:action_joystick_input",
"//frc971/input:drivetrain_input",
"//frc971/input:joystick_input",
+ "//frc971/zeroing:wrap",
"//y2020:constants",
"//y2020/control_loops/drivetrain:drivetrain_base",
"//y2020/control_loops/superstructure:superstructure_goal_fbs",
diff --git a/y2020/actors/splines/target_aligned_1.json b/y2020/actors/splines/target_aligned_1.json
index 5a1d082..ff3a581 100644
--- a/y2020/actors/splines/target_aligned_1.json
+++ b/y2020/actors/splines/target_aligned_1.json
@@ -1 +1 @@
-{"spline_count": 1, "spline_x": [3.340495531674842, 3.3229239110760016, 4.483964997108113, 6.058132772881773, 7.084092118999326, 8.012654807002901], "spline_y": [5.730681749413974, 6.367337321705237, 7.14109740201818, 7.627912651837696, 7.449284138182865, 7.4135680036085025], "constraints": [{"constraint_type": "LONGITUDINAL_ACCELERATION", "value": 3.0}, {"constraint_type": "LATERAL_ACCELERATION", "value": 2.0}, {"constraint_type": "VOLTAGE", "value": 10.0}]}
\ No newline at end of file
+{"spline_count": 1, "spline_x": [3.0719744897959185, 3.798077551020408, 5.082721428571428, 5.473700000000001, 6.981760204081634, 7.7916443877551025], "spline_y": [2.373798469387755, 2.373798469387755, 1.368425, 0.33512448979591836, 0.7261030612244898, 0.6981760204081633], "constraints": [{"constraint_type": "LONGITUDINAL_ACCELERATION", "value": 3.0}, {"constraint_type": "LATERAL_ACCELERATION", "value": 2.0}, {"constraint_type": "VOLTAGE", "value": 10.0}]}
\ No newline at end of file
diff --git a/y2020/actors/splines/target_aligned_2.json b/y2020/actors/splines/target_aligned_2.json
index bca8965..754ce52 100644
--- a/y2020/actors/splines/target_aligned_2.json
+++ b/y2020/actors/splines/target_aligned_2.json
@@ -1 +1 @@
-{"spline_count": 1, "spline_x": [8.02107576923077, 7.157915384615384, 6.042122692307692, 5.010540769230768, 4.2315911538461535, 3.368430769230769], "spline_y": [7.389495, 7.431600384615384, 6.652650769230769, 6.1473861538461545, 5.7684376923076925, 5.7473849999999995], "constraints": [{"constraint_type": "LONGITUDINAL_ACCELERATION", "value": 3.0}, {"constraint_type": "LATERAL_ACCELERATION", "value": 2.0}, {"constraint_type": "VOLTAGE", "value": 10.0}]}
\ No newline at end of file
+{"spline_count": 1, "spline_x": [7.987133673469388, 5.613335204081633, 5.613335204081633, 4.719669897959184, 3.630515306122449, 3.099901530612245], "spline_y": [0.6981760204081633, 0.6423219387755102, 0.6143948979591837, 1.2846438775510205, 1.9269658163265306, 2.4017255102040815], "constraints": [{"constraint_type": "LONGITUDINAL_ACCELERATION", "value": 3}, {"constraint_type": "LATERAL_ACCELERATION", "value": 2}, {"constraint_type": "VOLTAGE", "value": 10}]}
\ No newline at end of file
diff --git a/y2020/actors/splines/target_offset_1.json b/y2020/actors/splines/target_offset_1.json
index 7b56061..de8a62d 100644
--- a/y2020/actors/splines/target_offset_1.json
+++ b/y2020/actors/splines/target_offset_1.json
@@ -1 +1 @@
-{"spline_count": 1, "spline_x": [3.31276940587483, 3.2907022466964535, 4.149941560222061, 3.9732949238578668, 4.6679269035533, 5.751552791878172], "spline_y": [2.721120947225566, 1.4302342949805793, 1.6475505418770833, 1.4726197969543149, 0.8891289340101522, 0.8335583756345177], "constraints": [{"constraint_type": "LONGITUDINAL_ACCELERATION", "value": 3.0}, {"constraint_type": "LATERAL_ACCELERATION", "value": 2.0}, {"constraint_type": "VOLTAGE", "value": 10.0}]}
\ No newline at end of file
+{"spline_count": 1, "spline_x": [3.1278285714285716, 3.1278285714285716, 3.686369387755104, 4.859305102040815, 5.4178459183673455, 5.948459693877551], "spline_y": [5.641262244897959, 6.255657142857143, 6.7304168367346975, 7.4565198979591845, 7.4565198979591845, 7.428592857142857], "constraints": [{"constraint_type": "LONGITUDINAL_ACCELERATION", "value": 3}, {"constraint_type": "LATERAL_ACCELERATION", "value": 2}, {"constraint_type": "VOLTAGE", "value": 10}]}
\ No newline at end of file
diff --git a/y2020/actors/splines/target_offset_2.json b/y2020/actors/splines/target_offset_2.json
index 6292a7f..4ead529 100644
--- a/y2020/actors/splines/target_offset_2.json
+++ b/y2020/actors/splines/target_offset_2.json
@@ -1 +1 @@
-{"spline_count": 1, "spline_x": [5.709874873096447, 4.5984637055837565, 4.112221319796954, 3.8482611675126903, 3.7232274111675125, 3.3481261421319797], "spline_y": [0.8057730964467005, 0.7779878172588832, 1.4170492385786801, 2.875776395939086, 4.167791878172588, 5.862693908629441], "constraints": [{"constraint_type": "LONGITUDINAL_ACCELERATION", "value": 3.0}, {"constraint_type": "LATERAL_ACCELERATION", "value": 2.0}, {"constraint_type": "VOLTAGE", "value": 10.0}]}
\ No newline at end of file
+{"spline_count": 1, "spline_x": [3.1836826530612243, 3.2953908163265306, 3.602588265306122, 4.635888775510204, 5.250283673469387, 5.948459693877551], "spline_y": [2.4575795918367347, 3.0161204081632653, 4.915159183673469, 6.590781632653061, 7.400665816326531, 7.428592857142857], "constraints": [{"constraint_type": "LONGITUDINAL_ACCELERATION", "value": 3}, {"constraint_type": "LATERAL_ACCELERATION", "value": 2}, {"constraint_type": "VOLTAGE", "value": 10}]}
\ No newline at end of file
diff --git a/y2020/constants.cc b/y2020/constants.cc
index 740dc8c..a173a91 100644
--- a/y2020/constants.cc
+++ b/y2020/constants.cc
@@ -135,7 +135,7 @@
case Values::kCompTeamNumber:
intake->zeroing_constants.measured_absolute_position =
- 1.42977866919024 - Values::kIntakeZero();
+ 0.433936997731885 - Values::kIntakeZero();
turret->potentiometer_offset = 5.52519370141247 + 0.00853506822980376 +
0.0109413725126625 - 0.223719825811759 +
diff --git a/y2020/control_loops/drivetrain/drivetrain_main.cc b/y2020/control_loops/drivetrain/drivetrain_main.cc
index dec9118..24c876f 100644
--- a/y2020/control_loops/drivetrain/drivetrain_main.cc
+++ b/y2020/control_loops/drivetrain/drivetrain_main.cc
@@ -15,11 +15,13 @@
aos::configuration::ReadConfig("config.json");
::aos::ShmEventLoop event_loop(&config.message());
- ::y2020::control_loops::drivetrain::Localizer localizer(
- &event_loop, ::y2020::control_loops::drivetrain::GetDrivetrainConfig());
+ std::unique_ptr<::y2020::control_loops::drivetrain::Localizer> localizer =
+ std::make_unique<y2020::control_loops::drivetrain::Localizer>(
+ &event_loop,
+ ::y2020::control_loops::drivetrain::GetDrivetrainConfig());
std::unique_ptr<DrivetrainLoop> drivetrain = std::make_unique<DrivetrainLoop>(
::y2020::control_loops::drivetrain::GetDrivetrainConfig(), &event_loop,
- &localizer);
+ localizer.get());
event_loop.Run();
diff --git a/y2020/control_loops/drivetrain/localizer.cc b/y2020/control_loops/drivetrain/localizer.cc
index 2d31144..937d8c4 100644
--- a/y2020/control_loops/drivetrain/localizer.cc
+++ b/y2020/control_loops/drivetrain/localizer.cc
@@ -333,6 +333,13 @@
noises *= 1.0 + std::abs((right_velocity() - left_velocity()) /
(2.0 * dt_config_.robot_radius) +
(is_turret ? turret_data.velocity : 0.0));
+
+ // Pay less attention to cameras that aren't actually on the turret, since they
+ // are less useful when it comes to actually making shots.
+ if (!is_turret) {
+ noises *= 3.0;
+ }
+
Eigen::Matrix3f R = Eigen::Matrix3f::Zero();
R.diagonal() = noises.cwiseAbs2();
Eigen::Matrix<float, HybridEkf::kNOutputs, HybridEkf::kNStates> H;
diff --git a/y2020/control_loops/drivetrain/localizer_test.cc b/y2020/control_loops/drivetrain/localizer_test.cc
index aac8f75..8ccb55c 100644
--- a/y2020/control_loops/drivetrain/localizer_test.cc
+++ b/y2020/control_loops/drivetrain/localizer_test.cc
@@ -521,7 +521,7 @@
// Give the filters enough time to converge.
RunFor(chrono::seconds(10));
VerifyNearGoal(5e-2);
- EXPECT_TRUE(VerifyEstimatorAccurate(4e-2));
+ EXPECT_TRUE(VerifyEstimatorAccurate(0.1));
}
// Tests that we are able to handle a constant, non-zero turret angle.
@@ -592,7 +592,7 @@
RunFor(chrono::seconds(10));
VerifyNearGoal(5e-3);
- EXPECT_TRUE(VerifyEstimatorAccurate(1e-2));
+ EXPECT_TRUE(VerifyEstimatorAccurate(5e-2));
}
// Tests that we don't blow up if we stop getting updates for an extended period
diff --git a/y2020/control_loops/python/intake.py b/y2020/control_loops/python/intake.py
index de60b28..1035070 100644
--- a/y2020/control_loops/python/intake.py
+++ b/y2020/control_loops/python/intake.py
@@ -20,8 +20,8 @@
kIntake = angular_system.AngularSystemParams(
name='Intake',
motor=control_loop.BAG(),
- G=(12.0 / 24.0) * (1.0 / 7.0) * (1.0 / 7.0) * (16.0 / 32.0),
- J=6 * 0.139 * 0.139,
+ G=(12.0 / 24.0) * (1.0 / 5.0) * (1.0 / 5.0) * (16.0 / 32.0),
+ J=8 * 0.139 * 0.139,
q_pos=0.40,
q_vel=20.0,
kalman_q_pos=0.12,
diff --git a/y2020/control_loops/superstructure/BUILD b/y2020/control_loops/superstructure/BUILD
index 803a65f..6ee38ad 100644
--- a/y2020/control_loops/superstructure/BUILD
+++ b/y2020/control_loops/superstructure/BUILD
@@ -200,3 +200,16 @@
"//aos/network/www:proxy",
],
)
+
+cc_binary(
+ name = "superstructure_replay",
+ srcs = ["superstructure_replay.cc"],
+ deps = [
+ ":superstructure_lib",
+ "//aos:configuration",
+ "//aos:init",
+ "//aos/events:simulated_event_loop",
+ "//aos/events/logging:log_reader",
+ "//aos/network:team_number",
+ ],
+)
diff --git a/y2020/control_loops/superstructure/shooter/flywheel_controller.h b/y2020/control_loops/superstructure/shooter/flywheel_controller.h
index df881c5..ce0e8bb 100644
--- a/y2020/control_loops/superstructure/shooter/flywheel_controller.h
+++ b/y2020/control_loops/superstructure/shooter/flywheel_controller.h
@@ -66,7 +66,7 @@
ptrdiff_t history_position_ = 0;
// Average velocity logging.
- double avg_angular_velocity_;
+ double avg_angular_velocity_ = 0;
double last_goal_ = 0;
diff --git a/y2020/control_loops/superstructure/superstructure_replay.cc b/y2020/control_loops/superstructure/superstructure_replay.cc
new file mode 100644
index 0000000..0148523
--- /dev/null
+++ b/y2020/control_loops/superstructure/superstructure_replay.cc
@@ -0,0 +1,76 @@
+// This binary allows us to replay the drivetrain code over existing logfile,
+// primarily for use in testing changes to the localizer code.
+// When you run this code, it generates a new logfile with the data all
+// replayed, so that it can then be run through the plotting tool or analyzed
+// in some other way. The original drivetrain status data will be on the
+// /original/drivetrain channel.
+#include "aos/events/logging/log_reader.h"
+#include "aos/events/simulated_event_loop.h"
+#include "aos/init.h"
+#include "aos/json_to_flatbuffer.h"
+#include "aos/logging/log_message_generated.h"
+#include "aos/network/team_number.h"
+#include "gflags/gflags.h"
+#include "y2020/constants.h"
+#include "y2020/control_loops/superstructure/superstructure.h"
+
+DEFINE_int32(team, 971, "Team number to use for logfile replay.");
+
+int main(int argc, char **argv) {
+ aos::InitGoogle(&argc, &argv);
+
+ aos::network::OverrideTeamNumber(FLAGS_team);
+ y2020::constants::InitValues();
+
+ // open logfiles
+ aos::logger::LogReader reader(
+ aos::logger::SortParts(aos::logger::FindLogs(argc, argv)));
+ // TODO(james): Actually enforce not sending on the same buses as the logfile
+ // spews out.
+ reader.RemapLoggedChannel("/superstructure",
+ "y2020.control_loops.superstructure.Status");
+ reader.RemapLoggedChannel("/superstructure",
+ "y2020.control_loops.superstructure.Output");
+
+ aos::SimulatedEventLoopFactory factory(reader.configuration());
+ reader.Register(&factory);
+
+ aos::NodeEventLoopFactory *roborio =
+ factory.GetNodeEventLoopFactory("roborio");
+
+ roborio->OnStartup([roborio]() {
+ roborio->AlwaysStart<y2020::control_loops::superstructure::Superstructure>(
+ "superstructure");
+ });
+
+ std::unique_ptr<aos::EventLoop> print_loop = roborio->MakeEventLoop("print");
+ print_loop->SkipAosLog();
+ print_loop->MakeWatcher(
+ "/aos", [&print_loop](const aos::logging::LogMessageFbs &msg) {
+ LOG(INFO) << print_loop->context().monotonic_event_time << " "
+ << aos::FlatbufferToJson(&msg);
+ });
+ print_loop->MakeWatcher(
+ "/superstructure",
+ [&print_loop](
+ const y2020::control_loops::superstructure::Position &position) {
+ LOG(INFO) << print_loop->context().monotonic_event_time << " "
+ << aos::FlatbufferToJson(position.hood());
+ });
+ print_loop->MakeWatcher(
+ "/superstructure",
+ [&print_loop](
+ const y2020::control_loops::superstructure::Status &status) {
+ if (status.estopped()) {
+ LOG(INFO) << "Estopped";
+ }
+ LOG(INFO) << print_loop->context().monotonic_event_time << " "
+ << aos::FlatbufferToJson(status.hood());
+ });
+
+ factory.Run();
+
+ reader.Deregister();
+
+ return 0;
+}
diff --git a/y2020/control_loops/superstructure/superstructure_status.fbs b/y2020/control_loops/superstructure/superstructure_status.fbs
index 16f7445..4a977b8 100644
--- a/y2020/control_loops/superstructure/superstructure_status.fbs
+++ b/y2020/control_loops/superstructure/superstructure_status.fbs
@@ -60,6 +60,8 @@
// The current "shot distance." When shooting on the fly, this may be
// different from the static distance to the target.
shot_distance:double (id: 4);
+ // Amount the shot is off from being dead-straight relative to the inner port.
+ inner_port_angle:double (id: 5);
}
table Status {
diff --git a/y2020/control_loops/superstructure/turret/aiming.cc b/y2020/control_loops/superstructure/turret/aiming.cc
index 2cc2ce0..94047d2 100644
--- a/y2020/control_loops/superstructure/turret/aiming.cc
+++ b/y2020/control_loops/superstructure/turret/aiming.cc
@@ -48,7 +48,7 @@
// exactly perpendicular to the target. Larger numbers allow us to aim at the
// inner port more aggressively, at the risk of being more likely to miss the
// outer port entirely.
-constexpr double kMaxInnerPortAngle = 20.0 * M_PI / 180.0;
+constexpr double kMaxInnerPortAngle = 10.0 * M_PI / 180.0;
// Distance (in meters) from the edge of the field to the port, with some
// compensation to ensure that our definition of where the target is matches
@@ -164,7 +164,7 @@
const double xdot = linear_angular(0) * std::cos(status->theta());
const double ydot = linear_angular(0) * std::sin(status->theta());
- const double inner_port_angle = robot_pose_from_inner_port.heading();
+ inner_port_angle_ = robot_pose_from_inner_port.heading();
const double inner_port_distance = robot_pose_from_inner_port.rel_pos().x();
// Add a bit of hysteresis so that we don't jump between aiming for the inner
// and outer ports.
@@ -174,7 +174,7 @@
aiming_for_inner_port_ ? (kMinimumInnerPortShotDistance - 0.3)
: kMinimumInnerPortShotDistance;
aiming_for_inner_port_ =
- (std::abs(inner_port_angle) < max_inner_port_angle) &&
+ (std::abs(inner_port_angle_) < max_inner_port_angle) &&
(inner_port_distance > min_inner_port_distance);
// This code manages compensating the goal turret heading for the robot's
@@ -262,6 +262,7 @@
builder.add_turret_velocity(goal_.message().goal_velocity());
builder.add_aiming_for_inner_port(aiming_for_inner_port_);
builder.add_target_distance(target_distance_);
+ builder.add_inner_port_angle(inner_port_angle_);
builder.add_shot_distance(DistanceToGoal());
return builder.Finish();
}
diff --git a/y2020/control_loops/superstructure/turret/aiming.h b/y2020/control_loops/superstructure/turret/aiming.h
index 7f69d48..ed00972 100644
--- a/y2020/control_loops/superstructure/turret/aiming.h
+++ b/y2020/control_loops/superstructure/turret/aiming.h
@@ -69,6 +69,7 @@
double shot_distance_ = 0.0; // meters
// Real-world distance to the target.
double target_distance_ = 0.0; // meters
+ double inner_port_angle_ = 0.0; // radians
};
} // namespace turret
diff --git a/y2020/joystick_reader.cc b/y2020/joystick_reader.cc
index 34d3f54..9c8bae5 100644
--- a/y2020/joystick_reader.cc
+++ b/y2020/joystick_reader.cc
@@ -15,6 +15,7 @@
#include "frc971/input/driver_station_data.h"
#include "frc971/input/drivetrain_input.h"
#include "frc971/input/joystick_input.h"
+#include "frc971/zeroing/wrap.h"
#include "y2020/constants.h"
#include "y2020/control_loops/drivetrain/drivetrain_base.h"
#include "y2020/control_loops/superstructure/superstructure_goal_generated.h"
@@ -48,9 +49,12 @@
const ButtonLocation kFeedDriver(1, 2);
const ButtonLocation kIntakeExtend(3, 9);
const ButtonLocation kIntakeExtendDriver(1, 4);
+const ButtonLocation kRedLocalizerReset(3, 13);
+const ButtonLocation kBlueLocalizerReset(3, 14);
const ButtonLocation kIntakeIn(4, 4);
const ButtonLocation kSpit(4, 3);
const ButtonLocation kLocalizerReset(3, 8);
+const ButtonLocation kIntakeSlightlyOut(3, 7);
const ButtonLocation kWinch(3, 14);
@@ -76,7 +80,46 @@
AOS_LOG(INFO, "Auto ended, assuming disc and have piece\n");
}
+ void BlueResetLocalizer() {
+ auto builder = localizer_control_sender_.MakeBuilder();
+
+ frc971::control_loops::drivetrain::LocalizerControl::Builder
+ localizer_control_builder = builder.MakeBuilder<
+ frc971::control_loops::drivetrain::LocalizerControl>();
+ localizer_control_builder.add_x(7.4);
+ localizer_control_builder.add_y(-1.7);
+ localizer_control_builder.add_theta_uncertainty(10.0);
+ localizer_control_builder.add_theta(0.0);
+ localizer_control_builder.add_keep_current_theta(false);
+ if (!builder.Send(localizer_control_builder.Finish())) {
+ AOS_LOG(ERROR, "Failed to reset blue localizer.\n");
+ }
+ }
+
+ void RedResetLocalizer() {
+ auto builder = localizer_control_sender_.MakeBuilder();
+
+ frc971::control_loops::drivetrain::LocalizerControl::Builder
+ localizer_control_builder = builder.MakeBuilder<
+ frc971::control_loops::drivetrain::LocalizerControl>();
+ localizer_control_builder.add_x(-7.4);
+ localizer_control_builder.add_y(1.7);
+ localizer_control_builder.add_theta_uncertainty(10.0);
+ localizer_control_builder.add_theta(M_PI);
+ localizer_control_builder.add_keep_current_theta(false);
+ if (!builder.Send(localizer_control_builder.Finish())) {
+ AOS_LOG(ERROR, "Failed to reset red localizer.\n");
+ }
+ }
+
void ResetLocalizer() {
+ const frc971::control_loops::drivetrain::Status *drivetrain_status =
+ this->drivetrain_status();
+ if (drivetrain_status == nullptr) {
+ return;
+ }
+ // Get the current position
+ // Snap to heading.
auto builder = localizer_control_sender_.MakeBuilder();
// Start roughly in front of the red-team goal, robot pointed away from
@@ -84,10 +127,12 @@
frc971::control_loops::drivetrain::LocalizerControl::Builder
localizer_control_builder = builder.MakeBuilder<
frc971::control_loops::drivetrain::LocalizerControl>();
- localizer_control_builder.add_x(5.0);
- localizer_control_builder.add_y(-2.0);
- localizer_control_builder.add_theta(M_PI);
- localizer_control_builder.add_theta_uncertainty(0.01);
+ localizer_control_builder.add_x(drivetrain_status->x());
+ localizer_control_builder.add_y(drivetrain_status->y());
+ const double new_theta =
+ frc971::zeroing::Wrap(drivetrain_status->theta(), 0, M_PI);
+ localizer_control_builder.add_theta(new_theta);
+ localizer_control_builder.add_theta_uncertainty(10.0);
if (!builder.Send(localizer_control_builder.Finish())) {
AOS_LOG(ERROR, "Failed to reset localizer.\n");
}
@@ -169,16 +214,27 @@
preload_intake = true;
} else if (data.IsPressed(kSpit)) {
roller_speed = -6.0f;
+ } else if (data.IsPressed(kIntakeSlightlyOut)) {
+ intake_pos = -0.426585;
+ roller_speed = 6.0f;
+ preload_intake = true;
}
if (data.IsPressed(kWinch)) {
climber_speed = 12.0f;
}
- if (data.IsPressed(kLocalizerReset)) {
+ if (data.PosEdge(kLocalizerReset)) {
ResetLocalizer();
}
+ if (data.PosEdge(kRedLocalizerReset)) {
+ RedResetLocalizer();
+ }
+ if (data.PosEdge(kBlueLocalizerReset)) {
+ BlueResetLocalizer();
+ }
+
auto builder = superstructure_goal_sender_.MakeBuilder();
flatbuffers::Offset<superstructure::Goal> superstructure_goal_offset;
@@ -191,7 +247,7 @@
flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
intake_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
*builder.fbb(), intake_pos,
- CreateProfileParameters(*builder.fbb(), 10.0, 30.0));
+ CreateProfileParameters(*builder.fbb(), 20.0, 70.0));
flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
turret_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
diff --git a/y2020/vision/BUILD b/y2020/vision/BUILD
index 1b5fce7..b43829d 100644
--- a/y2020/vision/BUILD
+++ b/y2020/vision/BUILD
@@ -101,6 +101,7 @@
":vision_fbs",
"//aos:flatbuffers",
"//aos/events:event_loop",
+ "//aos/network:message_bridge_server_fbs",
"//aos/network:team_number",
"//frc971/control_loops:quaternion_utils",
"//third_party:opencv",
@@ -159,3 +160,27 @@
"//third_party:opencv",
],
)
+
+cc_binary(
+ name = "extrinsics_calibration",
+ srcs = [
+ "extrinsics_calibration.cc",
+ ],
+ data = [
+ "//y2020:config",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//y2020:__subpackages__"],
+ deps = [
+ ":charuco_lib",
+ "//aos:init",
+ "//aos/events:shm_event_loop",
+ "//aos/events/logging:log_reader",
+ "//frc971/control_loops/drivetrain:improved_down_estimator",
+ "//frc971/wpilib:imu_batch_fbs",
+ "//frc971/wpilib:imu_fbs",
+ "//third_party:opencv",
+ "@com_google_absl//absl/strings:str_format",
+ "@org_tuxfamily_eigen//:eigen",
+ ],
+)
diff --git a/y2020/vision/calibration.cc b/y2020/vision/calibration.cc
index 517e65c..80f1491 100644
--- a/y2020/vision/calibration.cc
+++ b/y2020/vision/calibration.cc
@@ -31,18 +31,19 @@
pi_number_(aos::network::ParsePiNumber(pi)),
charuco_extractor_(
event_loop, pi,
- [this](cv::Mat rgb_image, const double age_double,
+ [this](cv::Mat rgb_image, const aos::monotonic_clock::time_point eof,
std::vector<int> charuco_ids,
std::vector<cv::Point2f> charuco_corners, bool valid,
Eigen::Vector3d rvec_eigen, Eigen::Vector3d tvec_eigen) {
- HandleCharuco(rgb_image, age_double, charuco_ids, charuco_corners,
+ HandleCharuco(rgb_image, eof, charuco_ids, charuco_corners,
valid, rvec_eigen, tvec_eigen);
}) {
CHECK(pi_number_) << ": Invalid pi number " << pi
<< ", failed to parse pi number";
}
- void HandleCharuco(cv::Mat rgb_image, const double /*age_double*/,
+ void HandleCharuco(cv::Mat rgb_image,
+ const aos::monotonic_clock::time_point /*eof*/,
std::vector<int> charuco_ids,
std::vector<cv::Point2f> charuco_corners, bool valid,
Eigen::Vector3d /*rvec_eigen*/,
diff --git a/y2020/vision/camera_reader.cc b/y2020/vision/camera_reader.cc
index 1fd7f8c..9bc9ba9 100644
--- a/y2020/vision/camera_reader.cc
+++ b/y2020/vision/camera_reader.cc
@@ -90,6 +90,7 @@
const std::vector<cv::Point2f> &target_point_vector,
const std::vector<float> &target_radius_vector,
const std::vector<int> &training_image_indices,
+ const std::vector<int> &homography_feature_counts,
aos::Sender<sift::ImageMatchResult> *result_sender,
bool send_details);
@@ -229,6 +230,7 @@
const std::vector<cv::Point2f> &target_point_vector,
const std::vector<float> &target_radius_vector,
const std::vector<int> &training_image_indices,
+ const std::vector<int> &homography_feature_counts,
aos::Sender<sift::ImageMatchResult> *result_sender, bool send_details) {
auto builder = result_sender->MakeBuilder();
const auto camera_calibration_offset =
@@ -273,6 +275,8 @@
pose_builder.add_query_target_point_x(target_point_vector[i].x);
pose_builder.add_query_target_point_y(target_point_vector[i].y);
pose_builder.add_query_target_point_radius(target_radius_vector[i]);
+ pose_builder.add_homography_feature_count(homography_feature_counts[i]);
+ pose_builder.add_training_image_index(training_image_indices[i]);
camera_poses.emplace_back(pose_builder.Finish());
}
const auto camera_poses_offset = builder.fbb()->CreateVector(camera_poses);
@@ -372,6 +376,7 @@
std::vector<cv::Point2f> target_point_vector;
std::vector<float> target_radius_vector;
std::vector<int> training_image_indices;
+ std::vector<int> homography_feature_counts;
// Iterate through matches for each training image
for (size_t i = 0; i < per_image_matches.size(); ++i) {
@@ -390,10 +395,12 @@
cv::findHomography(per_image.training_points, per_image.query_points,
CV_RANSAC, 3.0, mask);
+ const int homography_feature_count = cv::countNonZero(mask);
// If mask doesn't have enough leftover matches, skip these matches
- if (cv::countNonZero(mask) < kMinimumMatchCount) {
+ if (homography_feature_count < kMinimumMatchCount) {
continue;
}
+ homography_feature_counts.push_back(homography_feature_count);
VLOG(2) << "Number of matches after homography: " << cv::countNonZero(mask)
<< "\n";
@@ -584,11 +591,13 @@
SendImageMatchResult(image, keypoints, descriptors, all_good_matches,
camera_target_list, field_camera_list,
target_point_vector, target_radius_vector,
- training_image_indices, &detailed_result_sender_, true);
+ training_image_indices, homography_feature_counts,
+ &detailed_result_sender_, true);
SendImageMatchResult(image, keypoints, descriptors, all_good_matches,
camera_target_list, field_camera_list,
target_point_vector, target_radius_vector,
- training_image_indices, &result_sender_, false);
+ training_image_indices, homography_feature_counts,
+ &result_sender_, false);
}
void CameraReader::ReadImage() {
diff --git a/y2020/vision/charuco_lib.cc b/y2020/vision/charuco_lib.cc
index c86663a..77e1ba9 100644
--- a/y2020/vision/charuco_lib.cc
+++ b/y2020/vision/charuco_lib.cc
@@ -28,6 +28,7 @@
namespace frc971 {
namespace vision {
namespace chrono = std::chrono;
+using aos::monotonic_clock;
CameraCalibration::CameraCalibration(
const absl::Span<const uint8_t> training_data_bfbs, std::string_view pi) {
@@ -84,14 +85,56 @@
<< " on " << team_number.value();
}
-ImageCallback::ImageCallback(aos::EventLoop *event_loop,
- std::string_view channel,
- std::function<void(cv::Mat, double)> &&fn)
- : event_loop_(event_loop), handle_image_(std::move(fn)) {
+ImageCallback::ImageCallback(
+ aos::EventLoop *event_loop, std::string_view channel,
+ std::function<void(cv::Mat, monotonic_clock::time_point)> &&fn)
+ : event_loop_(event_loop),
+ server_fetcher_(
+ event_loop_->MakeFetcher<aos::message_bridge::ServerStatistics>(
+ "/aos")),
+ source_node_(aos::configuration::GetNode(
+ event_loop_->configuration(),
+ event_loop_->GetChannel<CameraImage>(channel)
+ ->source_node()
+ ->string_view())),
+ handle_image_(std::move(fn)) {
event_loop_->MakeWatcher(channel, [this](const CameraImage &image) {
- const aos::monotonic_clock::duration age =
- event_loop_->monotonic_now() -
- event_loop_->context().monotonic_event_time;
+ const monotonic_clock::time_point eof_source_node =
+ monotonic_clock::time_point(
+ chrono::nanoseconds(image.monotonic_timestamp_ns()));
+ server_fetcher_.Fetch();
+ if (!server_fetcher_.get()) {
+ return;
+ }
+
+ chrono::nanoseconds offset{0};
+ if (source_node_ != event_loop_->node()) {
+ // If we are viewing this image from another node, convert to our
+ // monotonic clock.
+ const aos::message_bridge::ServerConnection *server_connection = nullptr;
+
+ for (const aos::message_bridge::ServerConnection *connection :
+ *server_fetcher_->connections()) {
+ CHECK(connection->has_node());
+ if (connection->node()->name()->string_view() ==
+ source_node_->name()->string_view()) {
+ server_connection = connection;
+ break;
+ }
+ }
+
+ CHECK(server_connection != nullptr) << ": Failed to find client";
+ if (!server_connection->has_monotonic_offset()) {
+ VLOG(1) << "No offset yet.";
+ return;
+ }
+ offset = chrono::nanoseconds(server_connection->monotonic_offset());
+ }
+
+ const monotonic_clock::time_point eof = eof_source_node - offset;
+
+ const monotonic_clock::duration age =
+ event_loop_->monotonic_now() - eof;
const double age_double =
std::chrono::duration_cast<std::chrono::duration<double>>(age).count();
if (age > std::chrono::milliseconds(100)) {
@@ -104,16 +147,17 @@
const cv::Size image_size(image.cols(), image.rows());
cv::Mat rgb_image(image_size, CV_8UC3);
cv::cvtColor(image_color_mat, rgb_image, CV_YUV2BGR_YUYV);
- handle_image_(rgb_image, age_double);
+ handle_image_(rgb_image, eof);
});
}
CharucoExtractor::CharucoExtractor(
aos::EventLoop *event_loop, std::string_view pi,
- std::function<void(cv::Mat, const double, std::vector<int>,
+ std::function<void(cv::Mat, monotonic_clock::time_point, std::vector<int>,
std::vector<cv::Point2f>, bool, Eigen::Vector3d,
Eigen::Vector3d)> &&fn)
- : calibration_(SiftTrainingData(), pi),
+ : event_loop_(event_loop),
+ calibration_(SiftTrainingData(), pi),
dictionary_(cv::aruco::getPredefinedDictionary(
FLAGS_large_board ? cv::aruco::DICT_5X5_250
: cv::aruco::DICT_6X6_250)),
@@ -136,8 +180,8 @@
image_callback_(
event_loop,
absl::StrCat("/pi", std::to_string(pi_number_.value()), "/camera"),
- [this](cv::Mat rgb_image, const double age_double) {
- HandleImage(rgb_image, age_double);
+ [this](cv::Mat rgb_image, const monotonic_clock::time_point eof) {
+ HandleImage(rgb_image, eof);
}),
handle_charuco_(std::move(fn)) {
LOG(INFO) << "Using " << (FLAGS_large_board ? "large" : "small")
@@ -158,7 +202,12 @@
LOG(INFO) << "Connecting to channel /pi" << pi_number_.value() << "/camera";
}
-void CharucoExtractor::HandleImage(cv::Mat rgb_image, const double age_double) {
+void CharucoExtractor::HandleImage(cv::Mat rgb_image,
+ const monotonic_clock::time_point eof) {
+ const double age_double =
+ std::chrono::duration_cast<std::chrono::duration<double>>(
+ event_loop_->monotonic_now() - eof)
+ .count();
std::vector<int> marker_ids;
std::vector<std::vector<cv::Point2f>> marker_corners;
@@ -167,6 +216,10 @@
std::vector<cv::Point2f> charuco_corners;
std::vector<int> charuco_ids;
+ bool valid = false;
+ Eigen::Vector3d rvec_eigen = Eigen::Vector3d::Zero();
+ Eigen::Vector3d tvec_eigen = Eigen::Vector3d::Zero();
+
// If at least one marker detected
if (marker_ids.size() >= FLAGS_min_targets) {
// Run everything twice, once with the calibration, and once
@@ -190,14 +243,12 @@
charuco_ids, cv::Scalar(255, 0, 0));
cv::Vec3d rvec, tvec;
- const bool valid = cv::aruco::estimatePoseCharucoBoard(
+ valid = cv::aruco::estimatePoseCharucoBoard(
charuco_corners_with_calibration, charuco_ids_with_calibration,
board_, camera_matrix_, dist_coeffs_, rvec, tvec);
// if charuco pose is valid
if (valid) {
- Eigen::Vector3d rvec_eigen;
- Eigen::Vector3d tvec_eigen;
cv::cv2eigen(rvec, rvec_eigen);
cv::cv2eigen(tvec, tvec_eigen);
@@ -218,30 +269,21 @@
cv::aruco::drawAxis(rgb_image, camera_matrix_, dist_coeffs_, rvec, tvec,
0.1);
-
- handle_charuco_(rgb_image, age_double, charuco_ids, charuco_corners,
-
- true, rvec_eigen, tvec_eigen);
} else {
LOG(INFO) << "Age: " << age_double << ", invalid pose";
- handle_charuco_(rgb_image, age_double, charuco_ids, charuco_corners,
-
- false, Eigen::Vector3d::Zero(),
- Eigen::Vector3d::Zero());
}
} else {
- LOG(INFO) << "Age: " << age_double << ", no charuco IDs.";
- handle_charuco_(rgb_image, age_double, charuco_ids, charuco_corners,
-
- false, Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero());
+ LOG(INFO) << "Age: " << age_double << ", not enough charuco IDs, got "
+ << charuco_ids.size() << ", needed " << FLAGS_min_targets;
}
} else {
- LOG(INFO) << "Age: " << age_double << ", no marker IDs";
+ LOG(INFO) << "Age: " << age_double << ", not enough marker IDs, got "
+ << marker_ids.size() << ", needed " << FLAGS_min_targets;
cv::aruco::drawDetectedMarkers(rgb_image, marker_corners, marker_ids);
-
- handle_charuco_(rgb_image, age_double, charuco_ids, charuco_corners, false,
- Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero());
}
+
+ handle_charuco_(rgb_image, eof, charuco_ids, charuco_corners, valid,
+ rvec_eigen, tvec_eigen);
}
} // namespace vision
diff --git a/y2020/vision/charuco_lib.h b/y2020/vision/charuco_lib.h
index e8dc3eb..a54bfca 100644
--- a/y2020/vision/charuco_lib.h
+++ b/y2020/vision/charuco_lib.h
@@ -11,6 +11,7 @@
#include "absl/types/span.h"
#include "aos/events/event_loop.h"
+#include "aos/network/message_bridge_server_generated.h"
#include "y2020/vision/sift/sift_generated.h"
#include "y2020/vision/sift/sift_training_generated.h"
@@ -45,12 +46,15 @@
// view the image.
class ImageCallback {
public:
- ImageCallback(aos::EventLoop *event_loop, std::string_view channel,
- std::function<void(cv::Mat, double)> &&fn);
+ ImageCallback(
+ aos::EventLoop *event_loop, std::string_view channel,
+ std::function<void(cv::Mat, aos::monotonic_clock::time_point)> &&fn);
private:
aos::EventLoop *event_loop_;
- std::function<void(cv::Mat, double)> handle_image_;
+ aos::Fetcher<aos::message_bridge::ServerStatistics> server_fetcher_;
+ const aos::Node *source_node_;
+ std::function<void(cv::Mat, aos::monotonic_clock::time_point)> handle_image_;
};
// Class which calls a callback each time an image arrives with the information
@@ -59,17 +63,18 @@
public:
// The callback takes the following arguments:
// cv::Mat -> image with overlays drawn on it.
- // const double -> Duration between when the image was captured and this
- // callback was called.
+ // monotonic_clock::time_point -> Time on this node when this image was
+ // captured.
// std::vector<int> -> charuco_ids
// std::vector<cv::Point2f> -> charuco_corners
// bool -> true if rvec/tvec is valid.
// Eigen::Vector3d -> rvec
// Eigen::Vector3d -> tvec
- CharucoExtractor(aos::EventLoop *event_loop, std::string_view pi,
- std::function<void(cv::Mat, const double, std::vector<int>,
- std::vector<cv::Point2f>, bool,
- Eigen::Vector3d, Eigen::Vector3d)> &&fn);
+ CharucoExtractor(
+ aos::EventLoop *event_loop, std::string_view pi,
+ std::function<void(cv::Mat, aos::monotonic_clock::time_point,
+ std::vector<int>, std::vector<cv::Point2f>, bool,
+ Eigen::Vector3d, Eigen::Vector3d)> &&fn);
// Returns the aruco dictionary in use.
cv::Ptr<cv::aruco::Dictionary> dictionary() const { return dictionary_; }
@@ -82,10 +87,10 @@
const cv::Mat dist_coeffs() const { return dist_coeffs_; }
private:
- // Handles the image by detecting the charuco board in it and calling the
- // callback with the extracted board corners, corresponding IDs, and pose.
- void HandleImage(cv::Mat rgb_image, const double age_double);
+ // Handles the image by detecting the charuco board in it.
+ void HandleImage(cv::Mat rgb_image, aos::monotonic_clock::time_point eof);
+ aos::EventLoop *event_loop_;
CameraCalibration calibration_;
cv::Ptr<cv::aruco::Dictionary> dictionary_;
@@ -100,9 +105,9 @@
ImageCallback image_callback_;
// Function to call.
- std::function<void(cv::Mat, const double, std::vector<int>,
- std::vector<cv::Point2f>, bool, Eigen::Vector3d,
- Eigen::Vector3d)>
+ std::function<void(cv::Mat, aos::monotonic_clock::time_point,
+ std::vector<int>, std::vector<cv::Point2f>, bool,
+ Eigen::Vector3d, Eigen::Vector3d)>
handle_charuco_;
};
diff --git a/y2020/vision/extrinsics_calibration.cc b/y2020/vision/extrinsics_calibration.cc
new file mode 100644
index 0000000..e775e3e
--- /dev/null
+++ b/y2020/vision/extrinsics_calibration.cc
@@ -0,0 +1,329 @@
+#include <opencv2/aruco/charuco.hpp>
+#include <opencv2/calib3d.hpp>
+#include <opencv2/features2d.hpp>
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/imgproc.hpp>
+#include "Eigen/Dense"
+#include "Eigen/Geometry"
+
+#include "frc971/wpilib/imu_batch_generated.h"
+#include "absl/strings/str_format.h"
+#include "frc971/control_loops/quaternion_utils.h"
+#include "y2020/vision/charuco_lib.h"
+#include "aos/events/logging/log_reader.h"
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "aos/network/team_number.h"
+#include "aos/time/time.h"
+#include "aos/util/file.h"
+#include "frc971/control_loops/drivetrain/improved_down_estimator.h"
+#include "y2020/vision/sift/sift_generated.h"
+#include "y2020/vision/sift/sift_training_generated.h"
+#include "y2020/vision/tools/python_code/sift_training_data.h"
+#include "y2020/vision/vision_generated.h"
+#include "y2020/vision/charuco_lib.h"
+
+DEFINE_string(config, "config.json", "Path to the config file to use.");
+DEFINE_string(pi, "pi-7971-2", "Pi name to calibrate.");
+DEFINE_bool(display_undistorted, false,
+ "If true, display the undistorted image.");
+
+namespace frc971 {
+namespace vision {
+namespace chrono = std::chrono;
+using aos::distributed_clock;
+using aos::monotonic_clock;
+
+// Class to both accumulate and replay camera and IMU data in time order.
+class CalibrationData {
+ public:
+ CalibrationData()
+ : x_hat_(Eigen::Matrix<double, 9, 1>::Zero()),
+ q_(Eigen::Matrix<double, 9, 9>::Zero()) {}
+
+ // Adds an IMU point to the list at the provided time.
+ void AddImu(distributed_clock::time_point distributed_now,
+ Eigen::Vector3d gyro, Eigen::Vector3d accel) {
+ imu_points_.emplace_back(distributed_now, std::make_pair(gyro, accel));
+ }
+
+ // Adds a camera/charuco detection to the list at the provided time.
+ void AddCharuco(distributed_clock::time_point distributed_now,
+ Eigen::Vector3d rvec, Eigen::Vector3d tvec) {
+ rot_trans_points_.emplace_back(distributed_now, std::make_pair(rvec, tvec));
+ }
+
+ // Processes the data points by calling UpdateCamera and UpdateIMU in time
+ // order.
+ void ReviewData() {
+ size_t next_imu_point = 0;
+ size_t next_camera_point = 0;
+ while (true) {
+ if (next_imu_point != imu_points_.size()) {
+ // There aren't that many combinations, so just brute force them all
+ // rather than being too clever.
+ if (next_camera_point != rot_trans_points_.size()) {
+ if (imu_points_[next_imu_point].first >
+ rot_trans_points_[next_camera_point].first) {
+ // Camera!
+ UpdateCamera(rot_trans_points_[next_camera_point].first,
+ rot_trans_points_[next_camera_point].second);
+ ++next_camera_point;
+ } else {
+ // IMU!
+ UpdateIMU(imu_points_[next_imu_point].first,
+ imu_points_[next_imu_point].second);
+ ++next_imu_point;
+ }
+ } else {
+ if (next_camera_point != rot_trans_points_.size()) {
+ // Camera!
+ UpdateCamera(rot_trans_points_[next_camera_point].first,
+ rot_trans_points_[next_camera_point].second);
+ ++next_camera_point;
+ } else {
+ // Nothing left for either list of points, so we are done.
+ break;
+ }
+ }
+ }
+ }
+ }
+
+ void UpdateCamera(distributed_clock::time_point t,
+ std::pair<Eigen::Vector3d, Eigen::Vector3d> rt) {
+ LOG(INFO) << t << " Camera " << rt.second.transpose();
+ }
+
+ void UpdateIMU(distributed_clock::time_point t,
+ std::pair<Eigen::Vector3d, Eigen::Vector3d> wa) {
+ LOG(INFO) << t << " IMU " << wa.first.transpose();
+ }
+
+ private:
+ // TODO(austin): Actually use these. Or make a new "callback" object which has these.
+ Eigen::Matrix<double, 9, 1> x_hat_;
+ Eigen::Matrix<double, 9, 9> q_;
+
+ // Proposed filter states:
+ // States:
+ // xyz position
+ // xyz velocity
+ // orientation rotation vector
+ //
+ // Inputs
+ // xyz accel
+ // angular rates
+ //
+ // Measurement:
+ // xyz position
+ // orientation rotation vector
+
+ std::vector<std::pair<distributed_clock::time_point,
+ std::pair<Eigen::Vector3d, Eigen::Vector3d>>>
+ imu_points_;
+
+ // Store pose samples as timestamp, along with
+ // pair of rotation, translation vectors
+ std::vector<std::pair<distributed_clock::time_point,
+ std::pair<Eigen::Vector3d, Eigen::Vector3d>>>
+ rot_trans_points_;
+};
+
+// Class to register image and IMU callbacks in AOS and route them to the
+// corresponding CalibrationData class.
+class Calibration {
+ public:
+ Calibration(aos::SimulatedEventLoopFactory *event_loop_factory,
+ aos::EventLoop *image_event_loop, aos::EventLoop *imu_event_loop,
+ std::string_view pi, CalibrationData *data)
+ : image_event_loop_(image_event_loop),
+ image_factory_(event_loop_factory->GetNodeEventLoopFactory(
+ image_event_loop_->node())),
+ imu_event_loop_(imu_event_loop),
+ imu_factory_(event_loop_factory->GetNodeEventLoopFactory(
+ imu_event_loop_->node())),
+ charuco_extractor_(
+ image_event_loop_, pi,
+ [this](cv::Mat rgb_image, monotonic_clock::time_point eof,
+ std::vector<int> charuco_ids,
+ std::vector<cv::Point2f> charuco_corners, bool valid,
+ Eigen::Vector3d rvec_eigen, Eigen::Vector3d tvec_eigen) {
+ HandleCharuco(rgb_image, eof, charuco_ids, charuco_corners, valid,
+ rvec_eigen, tvec_eigen);
+ }),
+ data_(data) {
+ imu_event_loop_->MakeWatcher(
+ "/drivetrain", [this](const frc971::IMUValuesBatch &imu) {
+ if (!imu.has_readings()) {
+ return;
+ }
+ for (const frc971::IMUValues *value : *imu.readings()) {
+ HandleIMU(value);
+ }
+ });
+ }
+
+ // Processes a charuco detection.
+ void HandleCharuco(cv::Mat rgb_image, const monotonic_clock::time_point eof,
+ std::vector<int> /*charuco_ids*/,
+ std::vector<cv::Point2f> /*charuco_corners*/, bool valid,
+ Eigen::Vector3d rvec_eigen, Eigen::Vector3d tvec_eigen) {
+ if (valid) {
+ Eigen::Quaternion<double> rotation(
+ frc971::controls::ToQuaternionFromRotationVector(rvec_eigen));
+ Eigen::Translation3d translation(tvec_eigen);
+
+ const Eigen::Affine3d board_to_camera = translation * rotation;
+ (void)board_to_camera;
+
+ // TODO(austin): Need a gravity vector input.
+ //
+ // TODO(austin): Need a state, covariance, and model.
+ //
+ // TODO(austin): Need to record all the values out of a log and run it
+ // as a batch run so we can feed it into ceres.
+
+ // LOG(INFO) << "rotation " << rotation.matrix();
+ // LOG(INFO) << "translation " << translation.matrix();
+ // Z -> up
+ // Y -> away from cameras 2 and 3
+ // X -> left
+ Eigen::Vector3d imu(last_value_.accelerometer_x,
+ last_value_.accelerometer_y,
+ last_value_.accelerometer_z);
+
+ // For cameras 2 and 3...
+ Eigen::Quaternion<double> imu_to_camera(
+ Eigen::AngleAxisd(-0.5 * M_PI, Eigen::Vector3d::UnitX()));
+
+ Eigen::Quaternion<double> board_to_world(
+ Eigen::AngleAxisd(0.5 * M_PI, Eigen::Vector3d::UnitX()));
+
+ Eigen::IOFormat HeavyFmt(Eigen::FullPrecision, 0, ", ", ",\n", "[", "]",
+ "[", "]");
+
+ LOG(INFO);
+ LOG(INFO) << "World Gravity "
+ << (board_to_world * rotation.inverse() * imu_to_camera * imu)
+ .transpose()
+ .format(HeavyFmt);
+ LOG(INFO) << "Board Gravity "
+ << (rotation.inverse() * imu_to_camera * imu)
+ .transpose()
+ .format(HeavyFmt);
+ LOG(INFO) << "Camera Gravity "
+ << (imu_to_camera * imu).transpose().format(HeavyFmt);
+ LOG(INFO) << "IMU Gravity " << imu.transpose().format(HeavyFmt);
+
+ const double age_double =
+ std::chrono::duration_cast<std::chrono::duration<double>>(
+ image_event_loop_->monotonic_now() - eof)
+ .count();
+ LOG(INFO) << std::fixed << std::setprecision(6) << "Age: " << age_double
+ << ", Pose is R:" << rvec_eigen.transpose().format(HeavyFmt)
+ << " T:" << tvec_eigen.transpose().format(HeavyFmt);
+
+ data_->AddCharuco(image_factory_->ToDistributedClock(eof), rvec_eigen,
+ tvec_eigen);
+ }
+
+ cv::imshow("Display", rgb_image);
+
+ if (FLAGS_display_undistorted) {
+ const cv::Size image_size(rgb_image.cols, rgb_image.rows);
+ cv::Mat undistorted_rgb_image(image_size, CV_8UC3);
+ cv::undistort(rgb_image, undistorted_rgb_image,
+ charuco_extractor_.camera_matrix(),
+ charuco_extractor_.dist_coeffs());
+
+ cv::imshow("Display undist", undistorted_rgb_image);
+ }
+
+ int keystroke = cv::waitKey(1);
+ if ((keystroke & 0xFF) == static_cast<int>('q')) {
+ // image_event_loop_->Exit();
+ }
+ }
+
+ // Processes an IMU reading.
+ void HandleIMU(const frc971::IMUValues *imu) {
+ VLOG(1) << "IMU " << imu;
+ imu->UnPackTo(&last_value_);
+ Eigen::Vector3d gyro(last_value_.gyro_x, last_value_.gyro_y,
+ last_value_.gyro_z);
+ Eigen::Vector3d accel(last_value_.accelerometer_x,
+ last_value_.accelerometer_y,
+ last_value_.accelerometer_z);
+
+ data_->AddImu(imu_factory_->ToDistributedClock(monotonic_clock::time_point(
+ chrono::nanoseconds(imu->monotonic_timestamp_ns()))),
+ gyro, accel);
+ }
+
+ frc971::IMUValuesT last_value_;
+
+ private:
+ aos::EventLoop *image_event_loop_;
+ aos::NodeEventLoopFactory *image_factory_;
+ aos::EventLoop *imu_event_loop_;
+ aos::NodeEventLoopFactory *imu_factory_;
+
+ CharucoExtractor charuco_extractor_;
+
+ CalibrationData *data_;
+};
+
+void Main(int argc, char **argv) {
+ CalibrationData data;
+
+ {
+ // Now, accumulate all the data into the data object.
+ aos::logger::LogReader reader(
+ aos::logger::SortParts(aos::logger::FindLogs(argc, argv)));
+
+ aos::SimulatedEventLoopFactory factory(reader.configuration());
+ reader.Register(&factory);
+
+ CHECK(aos::configuration::MultiNode(reader.configuration()));
+
+ // Find the nodes we care about.
+ const aos::Node *const roborio_node =
+ aos::configuration::GetNode(factory.configuration(), "roborio");
+
+ std::optional<uint16_t> pi_number = aos::network::ParsePiNumber(FLAGS_pi);
+ CHECK(pi_number);
+ LOG(INFO) << "Pi " << *pi_number;
+ const aos::Node *const pi_node = aos::configuration::GetNode(
+ factory.configuration(), absl::StrCat("pi", *pi_number));
+
+ LOG(INFO) << "roboRIO " << aos::FlatbufferToJson(roborio_node);
+ LOG(INFO) << "Pi " << aos::FlatbufferToJson(pi_node);
+
+ std::unique_ptr<aos::EventLoop> roborio_event_loop =
+ factory.MakeEventLoop("calibration", roborio_node);
+ std::unique_ptr<aos::EventLoop> pi_event_loop =
+ factory.MakeEventLoop("calibration", pi_node);
+
+ // Now, hook Calibration up to everything.
+ Calibration extractor(&factory, pi_event_loop.get(),
+ roborio_event_loop.get(), FLAGS_pi, &data);
+
+ factory.Run();
+
+ reader.Deregister();
+ }
+
+ LOG(INFO) << "Done with event_loop running";
+ // And now we have it, we can start processing it.
+ data.ReviewData();
+}
+
+} // namespace vision
+} // namespace frc971
+
+int main(int argc, char **argv) {
+ aos::InitGoogle(&argc, &argv);
+
+ frc971::vision::Main(argc, argv);
+}
diff --git a/y2020/vision/sift/sift.fbs b/y2020/vision/sift/sift.fbs
index 5c44858..427b91c 100644
--- a/y2020/vision/sift/sift.fbs
+++ b/y2020/vision/sift/sift.fbs
@@ -143,6 +143,10 @@
query_target_point_y:float (id: 4);
// Perceived radius of target circle
query_target_point_radius:float (id: 5);
+ // Number of features used in this match.
+ homography_feature_count:int (id: 6);
+ // Training image used for this pose.
+ training_image_index:int (id: 7);
}
table ImageMatchResult {
diff --git a/y2020/vision/tools/python_code/calib_files/calibration_pi-971-2_2021-10-16_18-02-43.755702843.json b/y2020/vision/tools/python_code/calib_files/calibration_pi-971-2_2021-10-16_18-02-43.755702843.json
new file mode 100644
index 0000000..a371824
--- /dev/null
+++ b/y2020/vision/tools/python_code/calib_files/calibration_pi-971-2_2021-10-16_18-02-43.755702843.json
@@ -0,0 +1,23 @@
+{
+ "node_name": "pi2",
+ "team_number": 971,
+ "intrinsics": [
+ 394.258514,
+ 0.0,
+ 290.99823,
+ 0.0,
+ 394.159424,
+ 230.590912,
+ 0.0,
+ 0.0,
+ 1.0
+ ],
+ "dist_coeffs": [
+ 0.15002,
+ -0.265071,
+ -0.000614,
+ -0.000109,
+ 0.099463
+ ],
+ "calibration_timestamp": 1634432563755702843
+}
diff --git a/y2020/vision/tools/python_code/calib_files/calibration_pi-971-3_2020-08-20_14-51-41.569498021.json b/y2020/vision/tools/python_code/calib_files/calibration_pi-971-3_2020-08-20_14-51-41.569498021.json
new file mode 100644
index 0000000..6e23055
--- /dev/null
+++ b/y2020/vision/tools/python_code/calib_files/calibration_pi-971-3_2020-08-20_14-51-41.569498021.json
@@ -0,0 +1,23 @@
+{
+ "node_name": "pi3",
+ "team_number": 971,
+ "intrinsics": [
+ 389.151123,
+ 0.0,
+ 340.096008,
+ 0.0,
+ 388.984833,
+ 245.953568,
+ 0.0,
+ 0.0,
+ 1.0
+ ],
+ "dist_coeffs": [
+ 0.143287,
+ -0.24945,
+ -0.000582,
+ -0.000178,
+ 0.089156
+ ],
+ "calibration_timestamp": 1597931501569498021
+}
diff --git a/y2020/vision/tools/python_code/calib_files/calibration_pi-971-4_2020-08-20_12-02-59.767009242.json b/y2020/vision/tools/python_code/calib_files/calibration_pi-971-4_2020-08-20_12-02-59.767009242.json
new file mode 100644
index 0000000..4c5342c
--- /dev/null
+++ b/y2020/vision/tools/python_code/calib_files/calibration_pi-971-4_2020-08-20_12-02-59.767009242.json
@@ -0,0 +1,23 @@
+{
+ "node_name": "pi4",
+ "team_number": 971,
+ "intrinsics": [
+ 390.175201,
+ 0.0,
+ 291.752258,
+ 0.0,
+ 390.163025,
+ 208.110153,
+ 0.0,
+ 0.0,
+ 1.0
+ ],
+ "dist_coeffs": [
+ 0.146071,
+ -0.258493,
+ 0.000083,
+ 0.000091,
+ 0.097735
+ ],
+ "calibration_timestamp": 1597921379767009242
+}
diff --git a/y2020/vision/tools/python_code/camera_definition.py b/y2020/vision/tools/python_code/camera_definition.py
index 727cf70..b0ce5f3 100644
--- a/y2020/vision/tools/python_code/camera_definition.py
+++ b/y2020/vision/tools/python_code/camera_definition.py
@@ -34,6 +34,57 @@
self.timestamp = 0
+def compute_extrinsic(camera_pitch, T_camera, is_turret):
+ # Compute the extrinsic calibration based on pitch and translation
+ # Includes camera rotation from robot x,y,z to opencv (z, -x, -y)
+
+ # Also, handle extrinsics for the turret
+ # The basic camera pose is relative to the center, base of the turret
+ # TODO<Jim>: Maybe store these to .json files, like with intrinsics?
+ base_cam_ext = CameraExtrinsics()
+ turret_cam_ext = CameraExtrinsics()
+
+ camera_pitch_matrix = np.array(
+ [[np.cos(camera_pitch), 0.0, -np.sin(camera_pitch)], [0.0, 1.0, 0.0],
+ [np.sin(camera_pitch), 0.0,
+ np.cos(camera_pitch)]])
+
+ robot_to_camera_rotation = np.array([[0., 0., 1.], [-1, 0, 0], [0, -1.,
+ 0]])
+
+ if is_turret:
+ # Turret camera has default heading 180 deg away from the robot x
+ base_cam_ext.R = np.array([[-1.0, 0.0, 0.0], [0.0, -1.0, 0.0],
+ [0.0, 0.0, 1.0]])
+ base_cam_ext.T = np.array([0.0, 0.0, 0.0])
+ turret_cam_ext.R = camera_pitch_matrix @ robot_to_camera_rotation
+ turret_cam_ext.T = T_camera
+ else:
+ base_cam_ext.R = camera_pitch_matrix @ robot_to_camera_rotation
+ base_cam_ext.T = T_camera
+ turret_cam_ext = None
+
+ return base_cam_ext, turret_cam_ext
+
+
+def compute_extrinsic_by_pi(pi_number):
+ # Defaults for non-turret camera
+ camera_pitch = 20.0 * np.pi / 180.0
+ is_turret = False
+ # Default camera location to robot origin
+ T = np.array([0.0, 0.0, 0.0])
+
+ if pi_number == "pi1":
+ # This is the turret camera
+ camera_pitch = 34.0 * np.pi / 180.0
+ is_turret = True
+ T = np.array([7.5 * 0.0254, -5.5 * 0.0254, 41.0 * 0.0254])
+ elif pi_number == "pi2":
+ T = np.array([4.5 * 0.0254, 3.75 * 0.0254, 26.0 * 0.0254])
+
+ return compute_extrinsic(camera_pitch, T, is_turret)
+
+
def load_camera_definitions():
### CAMERA DEFINITIONS
# We only load in cameras that have a calibration file
@@ -43,36 +94,11 @@
# Or better yet, use //y2020/vision:calibration to calibrate the camera
# using a Charuco target board
- # Extrinsic definition
- # Camera rotation from robot x,y,z to opencv (z, -x, -y)
- # This is extrinsics for the turret camera
- # camera pose relative to center, base of the turret
- # TODO<Jim>: Need to implement per-camera calibration, like with intrinsics
- camera_pitch = 34.0 * np.pi / 180.0
- camera_pitch_matrix = np.matrix(
- [[np.cos(camera_pitch), 0.0, -np.sin(camera_pitch)], [0.0, 1.0, 0.0],
- [np.sin(camera_pitch), 0.0,
- np.cos(camera_pitch)]])
- turret_cam_ext = CameraExtrinsics()
- turret_cam_ext.R = np.array(
- camera_pitch_matrix *
- np.matrix([[0., 0., 1.], [-1, 0, 0], [0, -1., 0]]))
- turret_cam_ext.T = np.array([7.5 * 0.0254, -5.5 * 0.0254, 41.0 * 0.0254])
- default_cam_ext = CameraExtrinsics()
- default_cam_ext.R = np.array([[-1.0, 0.0, 0.0], [0.0, -1.0, 0.0],
- [0.0, 0.0, 1.0]])
- default_cam_ext.T = np.array([0.0, 0.0, 0.0])
-
- default_cam_params = CameraParameters()
- # Currently, all cameras have this same set of extrinsics
- default_cam_params.camera_ext = default_cam_ext
- default_cam_params.turret_ext = turret_cam_ext
-
camera_list = []
dir_name = dtd.bazel_name_fix('calib_files')
glog.debug("Searching for calibration files in " + dir_name)
- for filename in os.listdir(dir_name):
+ for filename in sorted(os.listdir(dir_name)):
glog.debug("Inspecting %s", filename)
if ("cam-calib-int" in filename
or 'calibration' in filename) and filename.endswith(".json"):
@@ -104,11 +130,15 @@
glog.info("Found calib for " + node_name + ", team #" +
str(team_number))
- camera_base = copy.deepcopy(default_cam_params)
- camera_base.node_name = node_name
- camera_base.team_number = team_number
- camera_base.camera_int.camera_matrix = copy.copy(camera_matrix)
- camera_base.camera_int.dist_coeffs = copy.copy(dist_coeffs)
- camera_list.append(camera_base)
+
+ camera_params = CameraParameters()
+ camera_params.camera_ext, camera_params.turret_ext = compute_extrinsic_by_pi(
+ node_name)
+
+ camera_params.node_name = node_name
+ camera_params.team_number = team_number
+ camera_params.camera_int.camera_matrix = copy.copy(camera_matrix)
+ camera_params.camera_int.dist_coeffs = copy.copy(dist_coeffs)
+ camera_list.append(camera_params)
return camera_list
diff --git a/y2020/www/field.html b/y2020/www/field.html
index c75e7bf..14ebfb0 100644
--- a/y2020/www/field.html
+++ b/y2020/www/field.html
@@ -4,6 +4,53 @@
<link rel="stylesheet" href="styles.css">
</head>
<body>
+ <div id="field"> </div>
+ <div id="readouts">
+ <div>
+ <div> X </div>
+ <div id="x"> NA </div>
+ </div>
+ <div>
+ <div> Y </div>
+ <div id="y"> NA </div>
+ </div>
+ <div>
+ <div> Theta </div>
+ <div id="theta"> NA </div>
+ </div>
+ <div>
+ <div> Shot Distance </div>
+ <div id="shot_distance"> NA </div>
+ </div>
+ <div>
+ <div> Finisher </div>
+ <div id="finisher"> NA </div>
+ </div>
+ <div>
+ <div> Left Accelerator </div>
+ <div id="left_accelerator"> NA </div>
+ </div>
+ <div>
+ <div> Right Accelerator </div>
+ <div id="right_accelerator"> NA </div>
+ </div>
+ <div>
+ <div> Inner Port </div>
+ <div id="inner_port"> NA </div>
+ </div>
+ <div>
+ <div> Hood </div>
+ <div id="hood"> NA </div>
+ </div>
+ <div>
+ <div> Turret </div>
+ <div id="turret"> NA </div>
+ </div>
+ <div>
+ <div> Intake </div>
+ <div id="intake"> NA </div>
+ </div>
+ </div>
</body>
</html>
diff --git a/y2020/www/field_handler.ts b/y2020/www/field_handler.ts
index 1a0b56a..1cba7eb 100644
--- a/y2020/www/field_handler.ts
+++ b/y2020/www/field_handler.ts
@@ -67,9 +67,20 @@
private imageMatchResult = new Map<string, ImageMatchResult>();
private drivetrainStatus: DrivetrainStatus|null = null;
private superstructureStatus: SuperstructureStatus|null = null;
+ private x: HTMLDivElement = (document.getElementById('x') as HTMLDivElement);
+ private y: HTMLDivElement = (document.getElementById('y') as HTMLDivElement);
+ private theta: HTMLDivElement = (document.getElementById('theta') as HTMLDivElement);
+ private shotDistance: HTMLDivElement = (document.getElementById('shot_distance') as HTMLDivElement);
+ private finisher: HTMLDivElement = (document.getElementById('finisher') as HTMLDivElement);
+ private leftAccelerator: HTMLDivElement = (document.getElementById('left_accelerator') as HTMLDivElement);
+ private rightAccelerator: HTMLDivElement = (document.getElementById('right_accelerator') as HTMLDivElement);
+ private innerPort: HTMLDivElement = (document.getElementById('inner_port') as HTMLDivElement);
+ private hood: HTMLDivElement = (document.getElementById('hood') as HTMLDivElement);
+ private turret: HTMLDivElement = (document.getElementById('turret') as HTMLDivElement);
+ private intake: HTMLDivElement = (document.getElementById('intake') as HTMLDivElement);
constructor(private readonly connection: Connection) {
- document.body.appendChild(this.canvas);
+ (document.getElementById('field') as HTMLElement).appendChild(this.canvas);
this.connection.addConfigHandler(() => {
// Go through and register handlers for both all the individual pis as
@@ -231,6 +242,22 @@
ctx.restore();
}
+ setZeroing(div: HTMLDivElement): void {
+ div.innerHTML = "zeroing";
+ div.classList.remove("faulted");
+ div.classList.add("zeroing");
+ }
+ setEstopped(div: HTMLDivElement): void {
+ div.innerHTML = "estopped";
+ div.classList.add("faulted");
+ div.classList.remove("zeroing");
+ }
+ setValue(div: HTMLDivElement, val: Number): void {
+ div.innerHTML = val.toFixed(4);
+ div.classList.remove("faulted");
+ div.classList.remove("zeroing");
+ }
+
draw(): void {
this.reset();
this.drawField();
@@ -253,6 +280,50 @@
}
if (this.drivetrainStatus) {
+ if (!this.drivetrainStatus.zeroing().zeroed()) {
+ this.setZeroing(this.x);
+ this.setZeroing(this.y);
+ this.setZeroing(this.theta);
+ } else if (this.drivetrainStatus.zeroing().faulted()) {
+ this.setEstopped(this.x);
+ this.setEstopped(this.y);
+ this.setEstopped(this.theta);
+ } else {
+ this.setValue(this.x, this.drivetrainStatus.x());
+ this.setValue(this.y, this.drivetrainStatus.y());
+ this.setValue(this.theta, this.drivetrainStatus.theta());
+ }
+
+ this.shotDistance.innerHTML = this.superstructureStatus.aimer().shotDistance().toFixed(2);
+ this.finisher.innerHTML = this.superstructureStatus.shooter().finisher().angularVelocity().toFixed(2);
+ this.leftAccelerator.innerHTML = this.superstructureStatus.shooter().acceleratorLeft().angularVelocity().toFixed(2);
+ this.rightAccelerator.innerHTML = this.superstructureStatus.shooter().acceleratorRight().angularVelocity().toFixed(2);
+ if (this.superstructureStatus.aimer().aimingForInnerPort()) {
+ this.innerPort.innerHTML = "true";
+ } else {
+ this.innerPort.innerHTML = "false";
+ }
+ if (!this.superstructureStatus.hood().zeroed()) {
+ this.setZeroing(this.hood);
+ } else if (this.superstructureStatus.hood().estopped()) {
+ this.setEstopped(this.hood);
+ } else {
+ this.setValue(this.hood, this.superstructureStatus.hood().estimatorState().position());
+ }
+ if (!this.superstructureStatus.turret().zeroed()) {
+ this.setZeroing(this.turret);
+ } else if (this.superstructureStatus.turret().estopped()) {
+ this.setEstopped(this.turret);
+ } else {
+ this.setValue(this.turret, this.superstructureStatus.turret().estimatorState().position());
+ }
+ if (!this.superstructureStatus.intake().zeroed()) {
+ this.setZeroing(this.intake);
+ } else if (this.superstructureStatus.intake().estopped()) {
+ this.setEstopped(this.intake);
+ } else {
+ this.setValue(this.intake, this.superstructureStatus.intake().estimatorState().position());
+ }
this.drawRobot(
this.drivetrainStatus.x(), this.drivetrainStatus.y(),
this.drivetrainStatus.theta(),
diff --git a/y2020/www/styles.css b/y2020/www/styles.css
index 23ceb21..21bd239 100644
--- a/y2020/www/styles.css
+++ b/y2020/www/styles.css
@@ -3,3 +3,31 @@
border-bottom: 1px solid;
font-size: 24px;
}
+#field {
+ display: inline-block
+}
+#readouts {
+ display: inline-block;
+ vertical-align: top;
+ float: right;
+}
+
+#readouts > div {
+ display: table-row;
+ padding: 5px;
+}
+#readouts > div > div {
+ display: table-cell;
+ padding: 5px;
+ text-align: right;
+}
+
+.zeroing {
+ background-color: yellow;
+ border-radius: 10px;
+}
+
+.faulted {
+ background-color: red;
+ border-radius: 10px;
+}
diff --git a/y2020/y2020_pi_template.json b/y2020/y2020_pi_template.json
index 8b14bef..759cfee 100644
--- a/y2020/y2020_pi_template.json
+++ b/y2020/y2020_pi_template.json
@@ -126,28 +126,28 @@
"applications": [
{
"name": "message_bridge_client",
- "executable_name": "message_bridge_client.stripped",
+ "executable_name": "message_bridge_client",
"nodes": [
"pi{{ NUM }}"
]
},
{
"name": "message_bridge_server",
- "executable_name": "message_bridge_server.stripped",
+ "executable_name": "message_bridge_server",
"nodes": [
"pi{{ NUM }}"
]
},
{
"name": "web_proxy",
- "executable_name": "web_proxy_main.stripped",
+ "executable_name": "web_proxy_main",
"nodes": [
"pi{{ NUM }}"
]
},
{
"name": "camera_reader",
- "executable_name": "camera_reader.stripped",
+ "executable_name": "camera_reader",
"nodes": [
"pi{{ NUM }}"
]
diff --git a/y2020/y2020_roborio.json b/y2020/y2020_roborio.json
index 67916b6..4cdd035 100644
--- a/y2020/y2020_roborio.json
+++ b/y2020/y2020_roborio.json
@@ -332,70 +332,70 @@
"applications": [
{
"name": "drivetrain",
- "executable_name": "drivetrain.stripped",
+ "executable_name": "drivetrain",
"nodes": [
"roborio"
]
},
{
"name": "trajectory_generator",
- "executable_name": "trajectory_generator.stripped",
+ "executable_name": "trajectory_generator",
"nodes": [
"roborio"
]
},
{
"name": "superstructure",
- "executable_name": "superstructure.stripped",
+ "executable_name": "superstructure",
"nodes": [
"roborio"
]
},
{
"name": "joystick_reader",
- "executable_name": "joystick_reader.stripped",
+ "executable_name": "joystick_reader",
"nodes": [
"roborio"
]
},
{
"name": "wpilib_interface",
- "executable_name": "wpilib_interface.stripped",
+ "executable_name": "wpilib_interface",
"nodes": [
"roborio"
]
},
{
"name": "autonomous_action",
- "executable_name": "autonomous_action.stripped",
+ "executable_name": "autonomous_action",
"nodes": [
"roborio"
]
},
{
"name": "web_proxy",
- "executable_name": "web_proxy_main.stripped",
+ "executable_name": "web_proxy_main",
"nodes": [
"roborio"
]
},
{
"name": "message_bridge_client",
- "executable_name": "message_bridge_client.stripped",
+ "executable_name": "message_bridge_client",
"nodes": [
"roborio"
]
},
{
"name": "message_bridge_server",
- "executable_name": "message_bridge_server.stripped",
+ "executable_name": "message_bridge_server",
"nodes": [
"roborio"
]
},
{
"name": "logger",
- "executable_name": "logger_main.stripped",
+ "executable_name": "logger_main",
"nodes": [
"roborio"
]
diff --git a/y2021_bot3/control_loops/superstructure/superstructure.cc b/y2021_bot3/control_loops/superstructure/superstructure.cc
index 22c7c44..fbedb22 100644
--- a/y2021_bot3/control_loops/superstructure/superstructure.cc
+++ b/y2021_bot3/control_loops/superstructure/superstructure.cc
@@ -31,6 +31,8 @@
std::clamp(unsafe_goal->intake_speed(), -12.0, 12.0);
output_struct.outtake_volts =
std::clamp(unsafe_goal->outtake_speed(), -12.0, 12.0);
+ output_struct.climber_volts =
+ std::clamp(unsafe_goal->climber_speed(), -12.0, 12.0);
output->Send(Output::Pack(*output->fbb(), &output_struct));
}
@@ -42,6 +44,7 @@
if (unsafe_goal != nullptr) {
status_builder.add_intake_speed(unsafe_goal->intake_speed());
status_builder.add_outtake_speed(unsafe_goal->outtake_speed());
+ status_builder.add_climber_speed(unsafe_goal->climber_speed());
}
status->Send(status_builder.Finish());
diff --git a/y2021_bot3/control_loops/superstructure/superstructure_goal.fbs b/y2021_bot3/control_loops/superstructure/superstructure_goal.fbs
index 718edca..e1e3fce 100644
--- a/y2021_bot3/control_loops/superstructure/superstructure_goal.fbs
+++ b/y2021_bot3/control_loops/superstructure/superstructure_goal.fbs
@@ -9,7 +9,8 @@
// Outtake speed for the outtake roller, positive number means it is outtaking balls
outtake_speed:double (id:1);
- // TODO: Add climber
+ // Positive is deploying climber and to climb; cannot run in reverse
+ climber_speed:double (id: 2);
}
diff --git a/y2021_bot3/control_loops/superstructure/superstructure_lib_test.cc b/y2021_bot3/control_loops/superstructure/superstructure_lib_test.cc
index cd031c3..a6b6353 100644
--- a/y2021_bot3/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2021_bot3/control_loops/superstructure/superstructure_lib_test.cc
@@ -43,15 +43,18 @@
}
void VerifyResults(double intake_voltage, double outtake_voltage,
- double intake_speed, double outtake_speed) {
+ double climber_voltage, double intake_speed,
+ double outtake_speed, double climber_speed) {
superstructure_output_fetcher_.Fetch();
superstructure_status_fetcher_.Fetch();
ASSERT_TRUE(superstructure_output_fetcher_.get() != nullptr);
ASSERT_TRUE(superstructure_status_fetcher_.get() != nullptr);
EXPECT_EQ(superstructure_output_fetcher_->intake_volts(), intake_voltage);
EXPECT_EQ(superstructure_output_fetcher_->outtake_volts(), outtake_voltage);
+ EXPECT_EQ(superstructure_output_fetcher_->climber_volts(), climber_voltage);
EXPECT_EQ(superstructure_status_fetcher_->intake_speed(), intake_speed);
EXPECT_EQ(superstructure_status_fetcher_->outtake_speed(), outtake_speed);
+ EXPECT_EQ(superstructure_status_fetcher_->climber_speed(), climber_speed);
EXPECT_EQ(superstructure_status_fetcher_->estopped(), false);
EXPECT_EQ(superstructure_status_fetcher_->zeroed(), true);
}
@@ -83,7 +86,7 @@
ASSERT_TRUE(builder.Send(goal_builder.Finish()));
SendPositionMessage();
RunFor(dt() * 2);
- VerifyResults(10.0, 0.0, 10.0, 0.0);
+ VerifyResults(10.0, 0.0, 0.0, 10.0, 0.0, 0.0);
}
{
@@ -92,10 +95,19 @@
goal_builder.add_outtake_speed(10.0);
ASSERT_TRUE(builder.Send(goal_builder.Finish()));
RunFor(dt() * 2);
- VerifyResults(0.0, 10.0, 0.0, 10.0);
+ VerifyResults(0.0, 10.0, 0.0, 0.0, 10.0, 0.0);
}
}
+TEST_F(SuperstructureTest, RunClimber) {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_climber_speed(4.0);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
+ RunFor(dt() * 2);
+ VerifyResults(0.0, 0.0, 4.0, 0.0, 0.0, 4.0);
+}
+
// Tests running both the intake and the outtake simultaneously
TEST_F(SuperstructureTest, RunIntakeAndOuttake) {
auto builder = superstructure_goal_sender_.MakeBuilder();
@@ -104,7 +116,7 @@
goal_builder.add_outtake_speed(5.0);
ASSERT_TRUE(builder.Send(goal_builder.Finish()));
RunFor(dt() * 2);
- VerifyResults(10.0, 5.0, 10.0, 5.0);
+ VerifyResults(10.0, 5.0, 0.0, 10.0, 5.0, 0.0);
}
// Tests for an invalid voltage (over 12 or under -12) to check that it defaults
@@ -115,9 +127,10 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_intake_speed(20.0);
goal_builder.add_outtake_speed(15.0);
+ goal_builder.add_climber_speed(18.0);
ASSERT_TRUE(builder.Send(goal_builder.Finish()));
RunFor(dt() * 2);
- VerifyResults(12.0, 12.0, 20.0, 15.0);
+ VerifyResults(12.0, 12.0, 12.0, 20.0, 15.0, 18.0);
}
{
@@ -125,9 +138,10 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_intake_speed(-20.0);
goal_builder.add_outtake_speed(-15.0);
+ goal_builder.add_climber_speed(-18.0);
ASSERT_TRUE(builder.Send(goal_builder.Finish()));
RunFor(dt() * 2);
- VerifyResults(-12.0, -12.0, -20.0, -15.0);
+ VerifyResults(-12.0, -12.0, -12.0, -20.0, -15.0, -18.0);
}
}
@@ -137,7 +151,7 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
ASSERT_TRUE(builder.Send(goal_builder.Finish()));
RunFor(dt() * 2);
- VerifyResults(0.0, 0.0, 0.0, 0.0);
+ VerifyResults(0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
}
// Tests that the robot behaves properly when disabled
@@ -146,10 +160,11 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_intake_speed(6.0);
goal_builder.add_outtake_speed(5.0);
+ goal_builder.add_climber_speed(4.0);
ASSERT_TRUE(builder.Send(goal_builder.Finish()));
SetEnabled(false);
RunFor(dt() * 2);
- VerifyResults(0.0, 0.0, 6.0, 5.0);
+ VerifyResults(0.0, 0.0, 0.0, 6.0, 5.0, 4.0);
}
} // namespace testing
diff --git a/y2021_bot3/control_loops/superstructure/superstructure_output.fbs b/y2021_bot3/control_loops/superstructure/superstructure_output.fbs
index 47b9476..69c7090 100644
--- a/y2021_bot3/control_loops/superstructure/superstructure_output.fbs
+++ b/y2021_bot3/control_loops/superstructure/superstructure_output.fbs
@@ -7,8 +7,8 @@
// Voltage for outtake motor, positive number is outtaking balls
outtake_volts:double (id:1);
- // TODO: Add climber
-
+ // Positive is deploying climber and to climb; cannot run in reverse
+ climber_volts:double (id:2);
}
root_type Output;
diff --git a/y2021_bot3/control_loops/superstructure/superstructure_status.fbs b/y2021_bot3/control_loops/superstructure/superstructure_status.fbs
index 4f94e76..a53d0a6 100644
--- a/y2021_bot3/control_loops/superstructure/superstructure_status.fbs
+++ b/y2021_bot3/control_loops/superstructure/superstructure_status.fbs
@@ -16,7 +16,8 @@
// Outtake speed for the outtake roller, positive number outtakes balls
outtake_speed:double (id:3);
- // TODO: Add climber
+ // Positive is deploying climber and to climb; cannot run in reverse
+ climber_speed:double (id: 4);
}
-root_type Status;
+root_type Status;
\ No newline at end of file