Merge changes I015a5024,I00d0cdf4,Iec91284f,Ib4a2eb50,Ib5c1ed8f
* changes:
Log the timestamp each part file is created too
Actually populate boot_uuids in the header
Move reboot and header responsibility into DataWriter
Make LogNamer copy a template header rather than modify an input
Wrap DetachedBufferWriter in an object for logging
diff --git a/aos/aos_cli_utils.cc b/aos/aos_cli_utils.cc
index e36c3d2..84ba1e4 100644
--- a/aos/aos_cli_utils.cc
+++ b/aos/aos_cli_utils.cc
@@ -32,7 +32,8 @@
bool CliUtilInfo::Initialize(
int *argc, char ***argv,
- std::function<bool(const aos::Channel *)> channel_filter) {
+ std::function<bool(const aos::Channel *)> channel_filter,
+ bool expect_args) {
// Don't generate failure output if the config doesn't exist while attempting
// to autocomplete.
if (struct stat file_stat;
@@ -44,66 +45,76 @@
return true;
}
- std::string channel_name;
- std::string message_type;
- if (*argc > 1) {
- channel_name = (*argv)[1];
- ShiftArgs(argc, argv);
- }
- if (*argc > 1) {
- message_type = (*argv)[1];
- ShiftArgs(argc, argv);
- }
-
config.emplace(aos::configuration::ReadConfig(FLAGS_config));
event_loop.emplace(&config->message());
event_loop->SkipTimingReport();
event_loop->SkipAosLog();
- if (FLAGS__bash_autocomplete) {
- Autocomplete(channel_name, message_type, channel_filter);
- return true;
- }
-
- const flatbuffers::Vector<flatbuffers::Offset<aos::Channel>> *const channels =
+ const flatbuffers::Vector<flatbuffers::Offset<aos::Channel>> *channels =
event_loop->configuration()->channels();
- if (channel_name.empty() && message_type.empty()) {
- std::cout << "Channels:\n";
+
+ do {
+ std::string channel_name;
+ std::string message_type;
+ if (*argc > 1) {
+ channel_name = (*argv)[1];
+ ShiftArgs(argc, argv);
+ }
+ if (*argc > 1) {
+ message_type = (*argv)[1];
+ ShiftArgs(argc, argv);
+ }
+
+ if (FLAGS__bash_autocomplete) {
+ Autocomplete(channel_name, message_type, channel_filter);
+ return true;
+ }
+
+ if (channel_name.empty() && message_type.empty()) {
+ std::cout << "Channels:\n";
+ for (const aos::Channel *channel : *channels) {
+ if (FLAGS_all || channel_filter(channel)) {
+ std::cout << channel->name()->c_str() << ' '
+ << channel->type()->c_str() << '\n';
+ }
+ }
+ return true;
+ }
+
+ std::vector<const aos::Channel *> found_channels_now;
+ bool found_exact = false;
for (const aos::Channel *channel : *channels) {
- if (FLAGS_all || channel_filter(channel)) {
- std::cout << channel->name()->c_str() << ' ' << channel->type()->c_str()
- << '\n';
+ if (!FLAGS_all && !channel_filter(channel)) {
+ continue;
}
- }
- return true;
- }
-
- bool found_exact = false;
- for (const aos::Channel *channel : *channels) {
- if (!FLAGS_all && !channel_filter(channel)) {
- continue;
- }
- if (channel->name()->c_str() != channel_name) {
- continue;
- }
- if (channel->type()->string_view() == message_type) {
- if (!found_exact) {
- found_channels.clear();
- found_exact = true;
+ if (channel->name()->c_str() != channel_name) {
+ continue;
}
- } else if (!found_exact && channel->type()->string_view().find(
- message_type) != std::string_view::npos) {
- } else {
- continue;
+ if (channel->type()->string_view() == message_type) {
+ if (!found_exact) {
+ found_channels_now.clear();
+ found_exact = true;
+ }
+ } else if (!found_exact && channel->type()->string_view().find(
+ message_type) != std::string_view::npos) {
+ } else {
+ continue;
+ }
+ found_channels_now.push_back(channel);
}
- found_channels.push_back(channel);
- }
- if (found_channels.empty()) {
- LOG(FATAL) << "Could not find any channels with the given name and type.";
- } else if (found_channels.size() > 1 && !message_type.empty()) {
- LOG(FATAL) << "Multiple channels found with same type";
- }
+ if (found_channels_now.empty()) {
+ LOG(FATAL)
+ << "Could not find any channels with the given name and type for "
+ << channel_name << " " << message_type;
+ } else if (found_channels_now.size() > 1 && !message_type.empty()) {
+ LOG(FATAL) << "Multiple channels found with same type for "
+ << channel_name << " " << message_type;
+ }
+ for (const aos::Channel *channel : found_channels_now) {
+ found_channels.push_back(channel);
+ }
+ } while (expect_args && *argc > 1);
return false;
}
diff --git a/aos/aos_cli_utils.h b/aos/aos_cli_utils.h
index e158737..962e0a0 100644
--- a/aos/aos_cli_utils.h
+++ b/aos/aos_cli_utils.h
@@ -13,7 +13,8 @@
// If this returns false, the other fields will be filled out appropriately.
// event_loop will be filled out before channel_filter is called.
bool Initialize(int *argc, char ***argv,
- std::function<bool(const aos::Channel *)> channel_filter);
+ std::function<bool(const aos::Channel *)> channel_filter,
+ bool expect_args);
std::optional<aos::FlatbufferDetachedBuffer<aos::Configuration>> config;
std::optional<aos::ShmEventLoop> event_loop;
diff --git a/aos/aos_dump.cc b/aos/aos_dump.cc
index e2467b3..477b6fb 100644
--- a/aos/aos_dump.cc
+++ b/aos/aos_dump.cc
@@ -70,11 +70,13 @@
aos::InitGoogle(&argc, &argv);
aos::CliUtilInfo cli_info;
- if (cli_info.Initialize(&argc, &argv,
- [&cli_info](const aos::Channel *channel) {
- return aos::configuration::ChannelIsReadableOnNode(
- channel, cli_info.event_loop->node());
- })) {
+ if (cli_info.Initialize(
+ &argc, &argv,
+ [&cli_info](const aos::Channel *channel) {
+ return aos::configuration::ChannelIsReadableOnNode(
+ channel, cli_info.event_loop->node());
+ },
+ true)) {
return 0;
}
diff --git a/aos/aos_send.cc b/aos/aos_send.cc
index d94418a..91ffb1c 100644
--- a/aos/aos_send.cc
+++ b/aos/aos_send.cc
@@ -19,11 +19,13 @@
aos::InitGoogle(&argc, &argv);
aos::CliUtilInfo cli_info;
- if (cli_info.Initialize(&argc, &argv,
- [&cli_info](const aos::Channel *channel) {
- return aos::configuration::ChannelIsSendableOnNode(
- channel, cli_info.event_loop->node());
- })) {
+ if (cli_info.Initialize(
+ &argc, &argv,
+ [&cli_info](const aos::Channel *channel) {
+ return aos::configuration::ChannelIsSendableOnNode(
+ channel, cli_info.event_loop->node());
+ },
+ false)) {
return 0;
}
if (cli_info.found_channels.size() > 1) {
diff --git a/aos/events/logging/buffer_encoder.cc b/aos/events/logging/buffer_encoder.cc
index 10a7ed1..6ef61d4 100644
--- a/aos/events/logging/buffer_encoder.cc
+++ b/aos/events/logging/buffer_encoder.cc
@@ -39,7 +39,7 @@
}
DummyDecoder::DummyDecoder(std::string_view filename)
- : fd_(open(std::string(filename).c_str(), O_RDONLY | O_CLOEXEC)) {
+ : filename_(filename), fd_(open(filename_.c_str(), O_RDONLY | O_CLOEXEC)) {
PCHECK(fd_ != -1) << ": Failed to open " << filename;
}
diff --git a/aos/events/logging/buffer_encoder.h b/aos/events/logging/buffer_encoder.h
index 1eddd00..235a49c 100644
--- a/aos/events/logging/buffer_encoder.h
+++ b/aos/events/logging/buffer_encoder.h
@@ -2,9 +2,8 @@
#define AOS_EVENTS_LOGGING_BUFFER_ENCODER_H_
#include "absl/types/span.h"
-#include "flatbuffers/flatbuffers.h"
-
#include "aos/events/logging/logger_generated.h"
+#include "flatbuffers/flatbuffers.h"
namespace aos::logger {
@@ -89,6 +88,9 @@
// Returns less than end-begin if all bytes have been read. Otherwise, this
// will always fill the whole range.
virtual size_t Read(uint8_t *begin, uint8_t *end) = 0;
+
+ // Returns the underlying filename, for debugging purposes.
+ virtual std::string_view filename() const = 0;
};
// Simply reads the contents of the file into the target buffer.
@@ -102,8 +104,11 @@
~DummyDecoder() override;
size_t Read(uint8_t *begin, uint8_t *end) final;
+ std::string_view filename() const final { return filename_; }
private:
+ const std::string filename_;
+
// File descriptor for the log file.
int fd_;
diff --git a/aos/events/logging/log_cat.cc b/aos/events/logging/log_cat.cc
index 342a491..a26eb49 100644
--- a/aos/events/logging/log_cat.cc
+++ b/aos/events/logging/log_cat.cc
@@ -37,6 +37,10 @@
DEFINE_bool(print, true,
"If true, actually print the messages. If false, discard them, "
"confirming they can be parsed.");
+DEFINE_uint64(
+ count, 0,
+ "If >0, log_cat will exit after printing this many messages. This "
+ "includes messages from before the start of the log if --fetch is set.");
DEFINE_bool(print_parts_only, false,
"If true, only print out the results of logfile sorting.");
DEFINE_bool(channels, false,
@@ -247,6 +251,8 @@
bool found_channel = false;
+ uint64_t message_print_counter = 0;
+
for (const aos::Node *node :
aos::configuration::GetNodes(event_loop_factory.configuration())) {
std::unique_ptr<aos::EventLoop> printer_event_loop =
@@ -309,10 +315,15 @@
}
printer_event_loop->MakeRawWatcher(
- channel, [channel, node_name, &builder](const aos::Context &context,
- const void * /*message*/) {
+ channel, [channel, node_name, &builder, &event_loop_factory,
+ &message_print_counter](const aos::Context &context,
+ const void * /*message*/) {
if (FLAGS_print) {
PrintMessage(node_name, channel, context, &builder);
+ ++message_print_counter;
+ if (FLAGS_count > 0 && message_print_counter >= FLAGS_count) {
+ event_loop_factory.Exit();
+ }
}
});
found_channel = true;
@@ -325,6 +336,12 @@
if (FLAGS_print) {
PrintMessage(message.node_name, message.fetcher->channel(),
message.fetcher->context(), &builder);
+ ++message_print_counter;
+ if (FLAGS_count > 0 && message_print_counter >= FLAGS_count) {
+ // We are done. Clean up and exit.
+ reader.Deregister();
+ return 0;
+ }
}
}
printer_event_loops.emplace_back(std::move(printer_event_loop));
diff --git a/aos/events/logging/logfile_utils.cc b/aos/events/logging/logfile_utils.cc
index cd2ceb8..4c19867 100644
--- a/aos/events/logging/logfile_utils.cc
+++ b/aos/events/logging/logfile_utils.cc
@@ -17,9 +17,9 @@
#include "glog/logging.h"
#if defined(__x86_64__)
-#define ENABLE_LZMA 1
+#define ENABLE_LZMA (!__has_feature(memory_sanitizer))
#elif defined(__aarch64__)
-#define ENABLE_LZMA 1
+#define ENABLE_LZMA (!__has_feature(memory_sanitizer))
#else
#define ENABLE_LZMA 0
#endif
@@ -317,15 +317,15 @@
}
SpanReader::SpanReader(std::string_view filename) : filename_(filename) {
- static const std::string_view kXz = ".xz";
+ decoder_ = std::make_unique<DummyDecoder>(filename);
+
+ static constexpr std::string_view kXz = ".xz";
if (filename.substr(filename.size() - kXz.size()) == kXz) {
#if ENABLE_LZMA
- decoder_ = std::make_unique<ThreadedLzmaDecoder>(filename);
+ decoder_ = std::make_unique<ThreadedLzmaDecoder>(std::move(decoder_));
#else
LOG(FATAL) << "Reading xz-compressed files not supported on this platform";
#endif
- } else {
- decoder_ = std::make_unique<DummyDecoder>(filename);
}
}
diff --git a/aos/events/logging/lzma_encoder.cc b/aos/events/logging/lzma_encoder.cc
index 60a203e..1024b91 100644
--- a/aos/events/logging/lzma_encoder.cc
+++ b/aos/events/logging/lzma_encoder.cc
@@ -149,8 +149,9 @@
total_bytes_ += last_avail_out - stream_.avail_out;
}
-LzmaDecoder::LzmaDecoder(std::string_view filename)
- : dummy_decoder_(filename), stream_(LZMA_STREAM_INIT), filename_(filename) {
+LzmaDecoder::LzmaDecoder(std::unique_ptr<DataDecoder> underlying_decoder)
+ : underlying_decoder_(std::move(underlying_decoder)),
+ stream_(LZMA_STREAM_INIT) {
compressed_data_.resize(kBufSize);
lzma_ret status =
@@ -174,8 +175,8 @@
while (stream_.avail_out > 0) {
if (action_ == LZMA_RUN && stream_.avail_in == 0) {
// Read more bytes from the file if we're all out.
- const size_t count =
- dummy_decoder_.Read(compressed_data_.begin(), compressed_data_.end());
+ const size_t count = underlying_decoder_->Read(compressed_data_.begin(),
+ compressed_data_.end());
if (count == 0) {
// No more data to read in the file, begin the finishing operation.
action_ = LZMA_FINISH;
@@ -196,17 +197,18 @@
// If we fail to decompress, give up. Return everything that has been
// produced so far.
- if (!LzmaCodeIsOk(status, filename_)) {
+ if (!LzmaCodeIsOk(status, filename())) {
finished_ = true;
- LOG(WARNING) << filename_ << " is truncated or corrupted.";
+ LOG(WARNING) << filename() << " is truncated or corrupted.";
return (end - begin) - stream_.avail_out;
}
}
return end - begin;
}
-ThreadedLzmaDecoder::ThreadedLzmaDecoder(std::string_view filename)
- : decoder_(filename), decode_thread_([this] {
+ThreadedLzmaDecoder::ThreadedLzmaDecoder(
+ std::unique_ptr<DataDecoder> underlying_decoder)
+ : decoder_(std::move(underlying_decoder)), decode_thread_([this] {
std::unique_lock lock(decode_mutex_);
while (true) {
// Wake if the queue is too small or we are finished.
diff --git a/aos/events/logging/lzma_encoder.h b/aos/events/logging/lzma_encoder.h
index 972ed6c..6696200 100644
--- a/aos/events/logging/lzma_encoder.h
+++ b/aos/events/logging/lzma_encoder.h
@@ -1,6 +1,7 @@
#ifndef AOS_EVENTS_LOGGING_LZMA_ENCODER_H_
#define AOS_EVENTS_LOGGING_LZMA_ENCODER_H_
+#include <string_view>
#include <condition_variable>
#include <mutex>
#include <thread>
@@ -51,7 +52,9 @@
// Decompresses data with liblzma.
class LzmaDecoder final : public DataDecoder {
public:
- explicit LzmaDecoder(std::string_view filename);
+ explicit LzmaDecoder(std::unique_ptr<DataDecoder> underlying_decoder);
+ explicit LzmaDecoder(std::string_view filename)
+ : LzmaDecoder(std::make_unique<DummyDecoder>(filename)) {}
LzmaDecoder(const LzmaDecoder &) = delete;
LzmaDecoder(LzmaDecoder &&other) = delete;
LzmaDecoder &operator=(const LzmaDecoder &) = delete;
@@ -59,6 +62,9 @@
~LzmaDecoder();
size_t Read(uint8_t *begin, uint8_t *end) final;
+ std::string_view filename() const final {
+ return underlying_decoder_->filename();
+ }
private:
// Size of temporary buffer to use.
@@ -67,7 +73,7 @@
// Temporary buffer for storing compressed data.
ResizeableBuffer compressed_data_;
// Used for reading data from the file.
- DummyDecoder dummy_decoder_;
+ std::unique_ptr<DataDecoder> underlying_decoder_;
// Stream for decompression.
lzma_stream stream_;
// The current action. This is LZMA_RUN until we've run out of data to read
@@ -76,9 +82,6 @@
// Flag that represents whether or not all the data from the file has been
// successfully decoded.
bool finished_ = false;
-
- // Filename we are decompressing.
- std::string filename_;
};
// Decompresses data with liblzma in a new thread, up to a maximum queue
@@ -86,7 +89,9 @@
// or block until more data is queued or the stream finishes.
class ThreadedLzmaDecoder : public DataDecoder {
public:
- explicit ThreadedLzmaDecoder(std::string_view filename);
+ explicit ThreadedLzmaDecoder(std::string_view filename)
+ : ThreadedLzmaDecoder(std::make_unique<DummyDecoder>(filename)) {}
+ explicit ThreadedLzmaDecoder(std::unique_ptr<DataDecoder> underlying_decoder);
ThreadedLzmaDecoder(const ThreadedLzmaDecoder &) = delete;
ThreadedLzmaDecoder &operator=(const ThreadedLzmaDecoder &) = delete;
@@ -94,6 +99,8 @@
size_t Read(uint8_t *begin, uint8_t *end) final;
+ std::string_view filename() const final { return decoder_.filename(); }
+
private:
static constexpr size_t kBufSize{256 * 1024};
static constexpr size_t kQueueSize{8};
diff --git a/aos/logging/log_namer.cc b/aos/logging/log_namer.cc
index 707fd08..dfebe44 100644
--- a/aos/logging/log_namer.cc
+++ b/aos/logging/log_namer.cc
@@ -69,11 +69,9 @@
if (asprintf(filename, "%s/%s-%03d", directory, basename, fileindex) == -1) {
PLOG(FATAL) << "couldn't create final name";
}
- LOG(INFO) << "Created log file (" << basename << "-" << fileindex
- << ") in directory (" << directory
- << "). Previous file "
- "was ("
- << previous << ").";
+ // Fix basename formatting.
+ LOG(INFO) << "Created log file (" << filename << "). Previous file was ("
+ << directory << "/" << previous << ").";
}
bool FoundThumbDrive(const char *path) {
diff --git a/aos/network/message_bridge_client_lib.cc b/aos/network/message_bridge_client_lib.cc
index 1044b2f..80a41d9 100644
--- a/aos/network/message_bridge_client_lib.cc
+++ b/aos/network/message_bridge_client_lib.cc
@@ -90,7 +90,6 @@
return fbb.Release();
}
-
} // namespace
SctpClientConnection::SctpClientConnection(
@@ -150,6 +149,9 @@
void SctpClientConnection::MessageReceived() {
// Dispatch the message to the correct receiver.
aos::unique_c_ptr<Message> message = client_.Read();
+ if (!message) {
+ return;
+ }
if (message->message_type == Message::kNotification) {
const union sctp_notification *snp =
@@ -228,9 +230,14 @@
VLOG(1) << "Got a message of size " << message->size;
CHECK_EQ(message->size, flatbuffers::GetPrefixedSize(message->data()) +
sizeof(flatbuffers::uoffset_t));
+ {
+ flatbuffers::Verifier verifier(message->data(), message->size);
+ CHECK(remote_data->Verify(verifier));
+ }
const int stream = message->header.rcvinfo.rcv_sid - kControlStreams();
- SctpClientChannelState *channel_state = &((*channels_)[stream_to_channel_[stream]]);
+ SctpClientChannelState *channel_state =
+ &((*channels_)[stream_to_channel_[stream]]);
if (remote_data->queue_index() == channel_state->last_queue_index &&
monotonic_clock::time_point(
diff --git a/aos/network/message_bridge_server_lib.cc b/aos/network/message_bridge_server_lib.cc
index c929e31..da925f5 100644
--- a/aos/network/message_bridge_server_lib.cc
+++ b/aos/network/message_bridge_server_lib.cc
@@ -90,10 +90,10 @@
if (logged_remotely) {
if (sent_count == 0) {
- VLOG(1) << "No clients, rejecting";
- HandleFailure(fbb.Release());
+ VLOG(1)
+ << "No clients, rejecting. TODO(austin): do backup logging to disk";
} else {
- sent_messages_.emplace_back(fbb.Release());
+ VLOG(1) << "TODO(austin): backup log to disk if this fails eventually";
}
} else {
VLOG(1) << "Not bothering to track this message since nobody cares.";
@@ -165,40 +165,8 @@
}
}
- while (sent_messages_.size() > 0u) {
- if (sent_messages_.begin()->message().monotonic_sent_time() ==
- message_header->monotonic_sent_time() &&
- sent_messages_.begin()->message().queue_index() ==
- message_header->queue_index()) {
- sent_messages_.pop_front();
- continue;
- }
-
- if (sent_messages_.begin()->message().monotonic_sent_time() <
- message_header->monotonic_sent_time()) {
- VLOG(1) << "Delivery looks wrong, rejecting";
- HandleFailure(std::move(sent_messages_.front()));
- sent_messages_.pop_front();
- continue;
- }
-
- break;
- }
-}
-
-void ChannelState::HandleFailure(
- SizePrefixedFlatbufferDetachedBuffer<RemoteData> &&message) {
- // TODO(austin): Put it in the log queue.
- if (VLOG_IS_ON(2)) {
- LOG(INFO) << "Failed to send " << FlatbufferToJson(message);
- } else if (VLOG_IS_ON(1)) {
- message.mutable_message()->clear_data();
- LOG(INFO) << "Failed to send " << FlatbufferToJson(message);
- }
-
- // Note: this may be really out of order when we avoid the queue... We
- // have the ones we know didn't make it immediately, and the ones which
- // time out eventually. Need to sort that out.
+ // TODO(austin): record success of preceding messages, and log to disk if we
+ // don't find this in the list.
}
void ChannelState::AddPeer(const Connection *connection, int node_index,
@@ -329,7 +297,6 @@
if (configuration::ChannelIsSendableOnNode(channel, event_loop_->node()) &&
channel->has_destination_nodes()) {
-
bool any_reliable = false;
for (const Connection *connection : *channel->destination_nodes()) {
if (connection->time_to_live() == 0) {
@@ -432,6 +399,9 @@
void MessageBridgeServer::MessageReceived() {
aos::unique_c_ptr<Message> message = server_.Read();
+ if (!message) {
+ return;
+ }
if (message->message_type == Message::kNotification) {
const union sctp_notification *snp =
@@ -472,6 +442,10 @@
if (message->header.rcvinfo.rcv_sid == kConnectStream()) {
// Control channel!
const Connect *connect = flatbuffers::GetRoot<Connect>(message->data());
+ {
+ flatbuffers::Verifier verifier(message->data(), message->size);
+ CHECK(connect->Verify(verifier));
+ }
VLOG(1) << FlatbufferToJson(connect);
// Account for the control channel and delivery times channel.
@@ -516,6 +490,10 @@
// Message delivery
const logger::MessageHeader *message_header =
flatbuffers::GetRoot<logger::MessageHeader>(message->data());
+ {
+ flatbuffers::Verifier verifier(message->data(), message->size);
+ CHECK(message_header->Verify(verifier));
+ }
CHECK_LT(message_header->channel_index(), channels_.size());
CHECK_NOTNULL(channels_[message_header->channel_index()])
diff --git a/aos/network/message_bridge_server_lib.h b/aos/network/message_bridge_server_lib.h
index 7ff4e3a..db6e6d9 100644
--- a/aos/network/message_bridge_server_lib.h
+++ b/aos/network/message_bridge_server_lib.h
@@ -88,18 +88,12 @@
uint32_t partial_deliveries,
MessageBridgeServerStatus *server_status);
- // Handles (by consuming) failure to deliver a message.
- void HandleFailure(
- SizePrefixedFlatbufferDetachedBuffer<RemoteData> &&message);
-
private:
const int channel_index_;
const Channel *const channel_;
std::vector<Peer> peers_;
- std::deque<SizePrefixedFlatbufferDetachedBuffer<RemoteData>> sent_messages_;
-
// A fetcher to use to send the last message when a node connects and is
// reliable.
std::unique_ptr<aos::RawFetcher> last_message_fetcher_;
diff --git a/aos/network/message_bridge_test.cc b/aos/network/message_bridge_test.cc
index 8222760..74d22c2 100644
--- a/aos/network/message_bridge_test.cc
+++ b/aos/network/message_bridge_test.cc
@@ -679,10 +679,12 @@
// Make sure the offset in one direction is less than a second.
EXPECT_GT(
- client_statistics_fetcher->connections()->Get(0)->monotonic_offset(), 0);
+ client_statistics_fetcher->connections()->Get(0)->monotonic_offset(), 0)
+ << aos::FlatbufferToJson(client_statistics_fetcher.get());
EXPECT_LT(
client_statistics_fetcher->connections()->Get(0)->monotonic_offset(),
- 1000000000);
+ 1000000000)
+ << aos::FlatbufferToJson(client_statistics_fetcher.get());
EXPECT_GE(pi1_server_statistics_count, 2);
EXPECT_GE(pi2_server_statistics_count, 2);
diff --git a/aos/network/sctp_client.cc b/aos/network/sctp_client.cc
index 9e77a84..33115be 100644
--- a/aos/network/sctp_client.cc
+++ b/aos/network/sctp_client.cc
@@ -19,115 +19,30 @@
SctpClient::SctpClient(std::string_view remote_host, int remote_port,
int streams, std::string_view local_host, int local_port)
: sockaddr_remote_(ResolveSocket(remote_host, remote_port)),
- sockaddr_local_(ResolveSocket(local_host, local_port)),
- fd_(socket(sockaddr_local_.ss_family, SOCK_SEQPACKET, IPPROTO_SCTP)) {
- LOG(INFO) << "socket(" << Family(sockaddr_local_)
- << ", SOCK_SEQPACKET, IPPROTOSCTP) = " << fd_;
- PCHECK(fd_ != -1);
-
- {
- // Per https://tools.ietf.org/html/rfc6458
- // Setting this to !0 allows event notifications to be interleaved
- // with data if enabled, and would have to be handled in the code.
- // Enabling interleaving would only matter during congestion, which
- // typically only happens during application startup.
- int interleaving = 0;
- PCHECK(setsockopt(fd_, IPPROTO_SCTP, SCTP_FRAGMENT_INTERLEAVE,
- &interleaving, sizeof(interleaving)) == 0);
- }
+ sockaddr_local_(ResolveSocket(local_host, local_port)) {
+ sctp_.OpenSocket(sockaddr_local_);
{
struct sctp_initmsg initmsg;
memset(&initmsg, 0, sizeof(struct sctp_initmsg));
initmsg.sinit_num_ostreams = streams;
initmsg.sinit_max_instreams = streams;
- PCHECK(setsockopt(fd_, IPPROTO_SCTP, SCTP_INITMSG, &initmsg,
+ PCHECK(setsockopt(fd(), IPPROTO_SCTP, SCTP_INITMSG, &initmsg,
sizeof(struct sctp_initmsg)) == 0);
}
{
- int on = 1;
- PCHECK(setsockopt(fd_, IPPROTO_SCTP, SCTP_RECVRCVINFO, &on, sizeof(int)) ==
- 0);
- }
- {
// Servers send promptly. Clients don't.
// TODO(austin): Revisit this assumption when we have time sync.
int on = 0;
- PCHECK(setsockopt(fd_, IPPROTO_SCTP, SCTP_NODELAY, &on, sizeof(int)) == 0);
+ PCHECK(setsockopt(fd(), IPPROTO_SCTP, SCTP_NODELAY, &on, sizeof(int)) == 0);
}
- {
- // TODO(austin): This is the old style registration... But, the sctp
- // stack out in the wild for linux is old and primitive.
- struct sctp_event_subscribe subscribe;
- memset(&subscribe, 0, sizeof(subscribe));
- subscribe.sctp_association_event = 1;
- subscribe.sctp_stream_change_event = 1;
- PCHECK(setsockopt(fd_, SOL_SCTP, SCTP_EVENTS, (char *)&subscribe,
- sizeof(subscribe)) == 0);
- }
-
- PCHECK(bind(fd_, (struct sockaddr *)&sockaddr_local_,
+ PCHECK(bind(fd(), (struct sockaddr *)&sockaddr_local_,
sockaddr_local_.ss_family == AF_INET6
? sizeof(struct sockaddr_in6)
: sizeof(struct sockaddr_in)) == 0);
- VLOG(1) << "bind(" << fd_ << ", " << Address(sockaddr_local_) << ")";
-}
-
-aos::unique_c_ptr<Message> SctpClient::Read() {
- return ReadSctpMessage(fd_, max_size_);
-}
-
-bool SctpClient::Send(int stream, std::string_view data, int time_to_live) {
- struct iovec iov;
- iov.iov_base = const_cast<char *>(data.data());
- iov.iov_len = data.size();
-
- struct msghdr outmsg;
- // Target to send to.
- outmsg.msg_name = &sockaddr_remote_;
- outmsg.msg_namelen = sizeof(struct sockaddr_storage);
- VLOG(1) << "Sending to " << Address(sockaddr_remote_);
-
- // Data to send.
- outmsg.msg_iov = &iov;
- outmsg.msg_iovlen = 1;
-
- // Build up the sndinfo message.
- char outcmsg[CMSG_SPACE(sizeof(struct sctp_sndrcvinfo))];
- outmsg.msg_control = outcmsg;
- outmsg.msg_controllen = sizeof(outcmsg);
- outmsg.msg_flags = 0;
-
- struct cmsghdr *cmsg = CMSG_FIRSTHDR(&outmsg);
- cmsg->cmsg_level = IPPROTO_SCTP;
- cmsg->cmsg_type = SCTP_SNDRCV;
- cmsg->cmsg_len = CMSG_LEN(sizeof(struct sctp_sndrcvinfo));
-
- outmsg.msg_controllen = cmsg->cmsg_len;
- struct sctp_sndrcvinfo *sinfo = (struct sctp_sndrcvinfo *)CMSG_DATA(cmsg);
- memset(sinfo, 0, sizeof(struct sctp_sndrcvinfo));
- sinfo->sinfo_ppid = rand();
- sinfo->sinfo_stream = stream;
- sinfo->sinfo_context = 19;
- sinfo->sinfo_flags = 0;
- sinfo->sinfo_timetolive = time_to_live;
-
- // And send.
- const ssize_t size = sendmsg(fd_, &outmsg, MSG_NOSIGNAL | MSG_DONTWAIT);
- if (size == -1) {
- if (errno != EPIPE && errno != EAGAIN && errno != ESHUTDOWN) {
- PCHECK(size == static_cast<ssize_t>(data.size()));
- } else {
- return false;
- }
- } else {
- CHECK_EQ(static_cast<ssize_t>(data.size()), size);
- }
-
- VLOG(1) << "Sent " << data.size();
- return true;
+ VLOG(1) << "bind(" << fd() << ", " << Address(sockaddr_local_) << ")";
}
void SctpClient::LogSctpStatus(sctp_assoc_t assoc_id) {
diff --git a/aos/network/sctp_client.h b/aos/network/sctp_client.h
index 806026c..9356612 100644
--- a/aos/network/sctp_client.h
+++ b/aos/network/sctp_client.h
@@ -18,18 +18,18 @@
SctpClient(std::string_view remote_host, int remote_port, int streams,
std::string_view local_host = "0.0.0.0", int local_port = 9971);
- ~SctpClient() {
- LOG(INFO) << "close(" << fd_ << ")";
- PCHECK(close(fd_) == 0);
- }
+ ~SctpClient() {}
// Receives the next packet from the remote.
- aos::unique_c_ptr<Message> Read();
+ aos::unique_c_ptr<Message> Read() { return sctp_.ReadMessage(); }
// Sends a block of data on a stream with a TTL.
- bool Send(int stream, std::string_view data, int time_to_live);
+ // TODO(austin): time_to_live should be a chrono::duration
+ bool Send(int stream, std::string_view data, int time_to_live) {
+ return sctp_.SendMessage(stream, data, time_to_live, sockaddr_remote_, 0);
+ }
- int fd() { return fd_; }
+ int fd() { return sctp_.fd(); }
// Enables the priority scheduler. This is a SCTP feature which lets us
// configure the priority per stream so that higher priority packets don't get
@@ -43,26 +43,12 @@
void LogSctpStatus(sctp_assoc_t assoc_id);
- void SetMaxSize(size_t max_size) {
- max_size_ = max_size;
- // Have the kernel give us a factor of 10 more. This lets us have more than
- // one full sized packet in flight.
- max_size = max_size * 10;
-
- CHECK_GE(ReadRMemMax(), max_size);
- CHECK_GE(ReadWMemMax(), max_size);
- PCHECK(setsockopt(fd_, SOL_SOCKET, SO_RCVBUF, &max_size,
- sizeof(max_size)) == 0);
- PCHECK(setsockopt(fd_, SOL_SOCKET, SO_SNDBUF, &max_size,
- sizeof(max_size)) == 0);
- }
+ void SetMaxSize(size_t max_size) { sctp_.SetMaxSize(max_size); }
private:
struct sockaddr_storage sockaddr_remote_;
struct sockaddr_storage sockaddr_local_;
- int fd_;
-
- size_t max_size_ = 1000;
+ SctpReadWrite sctp_;
};
} // namespace message_bridge
diff --git a/aos/network/sctp_lib.cc b/aos/network/sctp_lib.cc
index b522851..4651d82 100644
--- a/aos/network/sctp_lib.cc
+++ b/aos/network/sctp_lib.cc
@@ -8,6 +8,7 @@
#include <sys/types.h>
#include <unistd.h>
+#include <algorithm>
#include <string_view>
#include "aos/util/file.h"
@@ -165,8 +166,13 @@
status.sstat_assoc_id = assoc_id;
socklen_t size = sizeof(status);
- PCHECK(getsockopt(fd, SOL_SCTP, SCTP_STATUS,
- reinterpret_cast<void *>(&status), &size) == 0);
+ const int result = getsockopt(fd, SOL_SCTP, SCTP_STATUS,
+ reinterpret_cast<void *>(&status), &size);
+ if (result == -1 && errno == EINVAL) {
+ LOG(INFO) << "sctp_status) not associated";
+ return;
+ }
+ PCHECK(result == 0);
LOG(INFO) << "sctp_status) sstat_assoc_id:" << status.sstat_assoc_id
<< " sstat_state:" << status.sstat_state
@@ -180,86 +186,320 @@
<< " sstat_primary.spinfo_rto:" << status.sstat_primary.spinfo_rto;
}
-aos::unique_c_ptr<Message> ReadSctpMessage(int fd, size_t max_size) {
- char incmsg[CMSG_SPACE(sizeof(_sctp_cmsg_data_t))];
+void SctpReadWrite::OpenSocket(const struct sockaddr_storage &sockaddr_local) {
+ fd_ = socket(sockaddr_local.ss_family, SOCK_SEQPACKET, IPPROTO_SCTP);
+ PCHECK(fd_ != -1);
+ LOG(INFO) << "socket(" << Family(sockaddr_local)
+ << ", SOCK_SEQPACKET, IPPROTOSCTP) = " << fd_;
+ {
+ // Per https://tools.ietf.org/html/rfc6458
+ // Setting this to !0 allows event notifications to be interleaved
+ // with data if enabled. This typically only matters during congestion.
+ // However, Linux seems to interleave under memory pressure regardless of
+ // this being enabled, so we have to handle it in the code anyways, so might
+ // as well turn it on all the time.
+ // TODO(Brian): Change this to 2 once we have kernels that support it, and
+ // also address the TODO in ProcessNotification to match on all the
+ // necessary fields.
+ int interleaving = 1;
+ PCHECK(setsockopt(fd_, IPPROTO_SCTP, SCTP_FRAGMENT_INTERLEAVE,
+ &interleaving, sizeof(interleaving)) == 0);
+ }
+ {
+ // Enable recvinfo when a packet arrives.
+ int on = 1;
+ PCHECK(setsockopt(fd_, IPPROTO_SCTP, SCTP_RECVRCVINFO, &on, sizeof(int)) ==
+ 0);
+ }
+
+ {
+ // TODO(austin): This is the old style registration... But, the sctp
+ // stack out in the wild for linux is old and primitive.
+ struct sctp_event_subscribe subscribe;
+ memset(&subscribe, 0, sizeof(subscribe));
+ subscribe.sctp_association_event = 1;
+ subscribe.sctp_stream_change_event = 1;
+ subscribe.sctp_partial_delivery_event = 1;
+ PCHECK(setsockopt(fd(), SOL_SCTP, SCTP_EVENTS, (char *)&subscribe,
+ sizeof(subscribe)) == 0);
+ }
+
+ DoSetMaxSize();
+}
+
+bool SctpReadWrite::SendMessage(
+ int stream, std::string_view data, int time_to_live,
+ std::optional<struct sockaddr_storage> sockaddr_remote,
+ sctp_assoc_t snd_assoc_id) {
+ CHECK(fd_ != -1);
struct iovec iov;
- struct msghdr inmessage;
+ iov.iov_base = const_cast<char *>(data.data());
+ iov.iov_len = data.size();
- aos::unique_c_ptr<Message> result(
- reinterpret_cast<Message *>(malloc(sizeof(Message) + max_size + 1)));
- result->size = 0;
+ // Use the assoc_id for the destination instead of the msg_name.
+ struct msghdr outmsg;
+ if (sockaddr_remote) {
+ outmsg.msg_name = &*sockaddr_remote;
+ outmsg.msg_namelen = sizeof(*sockaddr_remote);
+ VLOG(1) << "Sending to " << Address(*sockaddr_remote);
+ } else {
+ outmsg.msg_namelen = 0;
+ }
- int count = 0;
- int last_flags = 0;
- for (count = 0; !(last_flags & MSG_EOR); count++) {
+ // Data to send.
+ outmsg.msg_iov = &iov;
+ outmsg.msg_iovlen = 1;
+
+ // Build up the sndinfo message.
+ char outcmsg[CMSG_SPACE(sizeof(struct sctp_sndrcvinfo))];
+ outmsg.msg_control = outcmsg;
+ outmsg.msg_controllen = sizeof(outcmsg);
+ outmsg.msg_flags = 0;
+
+ struct cmsghdr *cmsg = CMSG_FIRSTHDR(&outmsg);
+ cmsg->cmsg_level = IPPROTO_SCTP;
+ cmsg->cmsg_type = SCTP_SNDRCV;
+ cmsg->cmsg_len = CMSG_LEN(sizeof(struct sctp_sndrcvinfo));
+
+ struct sctp_sndrcvinfo *sinfo =
+ reinterpret_cast<struct sctp_sndrcvinfo *>(CMSG_DATA(cmsg));
+ memset(sinfo, 0, sizeof(struct sctp_sndrcvinfo));
+ sinfo->sinfo_ppid = ++send_ppid_;
+ sinfo->sinfo_stream = stream;
+ sinfo->sinfo_flags = 0;
+ sinfo->sinfo_assoc_id = snd_assoc_id;
+ sinfo->sinfo_timetolive = time_to_live;
+
+ // And send.
+ const ssize_t size = sendmsg(fd_, &outmsg, MSG_NOSIGNAL | MSG_DONTWAIT);
+ if (size == -1) {
+ if (errno == EPIPE || errno == EAGAIN || errno == ESHUTDOWN ||
+ errno == EINTR) {
+ return false;
+ }
+ PLOG(FATAL) << "sendmsg on sctp socket failed";
+ return false;
+ }
+ CHECK_EQ(static_cast<ssize_t>(data.size()), size);
+ VLOG(1) << "Sent " << data.size();
+ return true;
+}
+
+// We read each fragment into a fresh Message, because most of them won't be
+// fragmented. If we do end up with a fragment, then we copy the data out of it.
+aos::unique_c_ptr<Message> SctpReadWrite::ReadMessage() {
+ CHECK(fd_ != -1);
+
+ while (true) {
+ aos::unique_c_ptr<Message> result(
+ reinterpret_cast<Message *>(malloc(sizeof(Message) + max_size_ + 1)));
+
+ struct msghdr inmessage;
memset(&inmessage, 0, sizeof(struct msghdr));
- iov.iov_len = max_size + 1 - result->size;
- iov.iov_base = result->mutable_data() + result->size;
+ struct iovec iov;
+ iov.iov_len = max_size_ + 1;
+ iov.iov_base = result->mutable_data();
inmessage.msg_iov = &iov;
inmessage.msg_iovlen = 1;
+ char incmsg[CMSG_SPACE(sizeof(_sctp_cmsg_data_t))];
inmessage.msg_control = incmsg;
inmessage.msg_controllen = sizeof(incmsg);
inmessage.msg_namelen = sizeof(struct sockaddr_storage);
inmessage.msg_name = &result->sin;
- ssize_t size;
- PCHECK((size = recvmsg(fd, &inmessage, 0)) > 0);
-
- if (count > 0) {
- VLOG(1) << "Count: " << count;
- VLOG(1) << "Last msg_flags: " << last_flags;
- VLOG(1) << "msg_flags: " << inmessage.msg_flags;
- VLOG(1) << "Current size: " << result->size;
- VLOG(1) << "Received size: " << size;
- CHECK_EQ(MSG_NOTIFICATION & inmessage.msg_flags, MSG_NOTIFICATION & last_flags);
- }
-
- result->size += size;
- last_flags = inmessage.msg_flags;
-
- for (struct cmsghdr *scmsg = CMSG_FIRSTHDR(&inmessage); scmsg != NULL;
- scmsg = CMSG_NXTHDR(&inmessage, scmsg)) {
- switch (scmsg->cmsg_type) {
- case SCTP_RCVINFO: {
- struct sctp_rcvinfo *data = reinterpret_cast<struct sctp_rcvinfo *>(CMSG_DATA(scmsg));
- if (count > 0) {
- VLOG(1) << "Got sctp_rcvinfo on continued packet";
- CHECK_EQ(result->header.rcvinfo.rcv_sid, data->rcv_sid);
- CHECK_EQ(result->header.rcvinfo.rcv_ssn, data->rcv_ssn);
- CHECK_EQ(result->header.rcvinfo.rcv_ppid, data->rcv_ppid);
- CHECK_EQ(result->header.rcvinfo.rcv_assoc_id, data->rcv_assoc_id);
- }
- result->header.rcvinfo = *data;
- } break;
- default:
- LOG(INFO) << "\tUnknown type: " << scmsg->cmsg_type;
- break;
+ const ssize_t size = recvmsg(fd_, &inmessage, MSG_DONTWAIT);
+ if (size == -1) {
+ if (errno == EINTR || errno == EAGAIN || errno == EWOULDBLOCK) {
+ // These are all non-fatal failures indicating we should retry later.
+ return nullptr;
}
+ PLOG(FATAL) << "recvmsg on sctp socket " << fd_ << " failed";
}
- CHECK_NE(inmessage.msg_flags & MSG_CTRUNC, MSG_CTRUNC)
+ CHECK(!(inmessage.msg_flags & MSG_CTRUNC))
<< ": Control message truncated.";
- CHECK_LE(result->size, max_size) << ": Message overflowed buffer on stream "
- << result->header.rcvinfo.rcv_sid << ".";
- }
+ CHECK_LE(size, static_cast<ssize_t>(max_size_))
+ << ": Message overflowed buffer on stream "
+ << result->header.rcvinfo.rcv_sid << ".";
- result->partial_deliveries = count - 1;
- if (count > 1) {
- VLOG(1) << "Final count: " << count;
- VLOG(1) << "Final size: " << result->size;
- }
+ result->size = size;
+ if (MSG_NOTIFICATION & inmessage.msg_flags) {
+ result->message_type = Message::kNotification;
+ } else {
+ result->message_type = Message::kMessage;
+ }
+ result->partial_deliveries = 0;
- if ((MSG_NOTIFICATION & inmessage.msg_flags)) {
- result->message_type = Message::kNotification;
- } else {
- result->message_type = Message::kMessage;
+ {
+ bool found_rcvinfo = false;
+ for (struct cmsghdr *scmsg = CMSG_FIRSTHDR(&inmessage); scmsg != NULL;
+ scmsg = CMSG_NXTHDR(&inmessage, scmsg)) {
+ switch (scmsg->cmsg_type) {
+ case SCTP_RCVINFO: {
+ CHECK(!found_rcvinfo);
+ found_rcvinfo = true;
+ result->header.rcvinfo =
+ *reinterpret_cast<struct sctp_rcvinfo *>(CMSG_DATA(scmsg));
+ } break;
+ default:
+ LOG(INFO) << "\tUnknown type: " << scmsg->cmsg_type;
+ break;
+ }
+ }
+ CHECK_EQ(found_rcvinfo, result->message_type == Message::kMessage)
+ << ": Failed to find a SCTP_RCVINFO cmsghdr. flags: "
+ << inmessage.msg_flags;
+ }
+ if (result->message_type == Message::kNotification) {
+ // Notifications are never fragmented, just return it now.
+ CHECK(inmessage.msg_flags & MSG_EOR)
+ << ": Notifications should never be big enough to fragment";
+ if (ProcessNotification(result.get())) {
+ // We handled this notification internally, so don't pass it on.
+ return nullptr;
+ }
+ return result;
+ }
+
+ auto partial_message_iterator =
+ std::find_if(partial_messages_.begin(), partial_messages_.end(),
+ [&result](const aos::unique_c_ptr<Message> &candidate) {
+ return result->header.rcvinfo.rcv_sid ==
+ candidate->header.rcvinfo.rcv_sid &&
+ result->header.rcvinfo.rcv_ssn ==
+ candidate->header.rcvinfo.rcv_ssn &&
+ result->header.rcvinfo.rcv_assoc_id ==
+ candidate->header.rcvinfo.rcv_assoc_id;
+ });
+ if (partial_message_iterator != partial_messages_.end()) {
+ const aos::unique_c_ptr<Message> &partial_message =
+ *partial_message_iterator;
+ // Verify it's really part of the same message.
+ CHECK_EQ(partial_message->message_type, result->message_type)
+ << ": for " << result->header.rcvinfo.rcv_sid << ","
+ << result->header.rcvinfo.rcv_ssn << ","
+ << result->header.rcvinfo.rcv_assoc_id;
+ CHECK_EQ(partial_message->header.rcvinfo.rcv_ppid,
+ result->header.rcvinfo.rcv_ppid)
+ << ": for " << result->header.rcvinfo.rcv_sid << ","
+ << result->header.rcvinfo.rcv_ssn << ","
+ << result->header.rcvinfo.rcv_assoc_id;
+
+ // Now copy the data over and update the size.
+ CHECK_LE(partial_message->size + result->size, max_size_)
+ << ": Assembled fragments overflowed buffer on stream "
+ << result->header.rcvinfo.rcv_sid << ".";
+ memcpy(partial_message->mutable_data() + partial_message->size,
+ result->data(), result->size);
+ ++partial_message->partial_deliveries;
+ VLOG(1) << "Merged fragment of " << result->size << " after "
+ << partial_message->size << ", had "
+ << partial_message->partial_deliveries
+ << ", for: " << result->header.rcvinfo.rcv_sid << ","
+ << result->header.rcvinfo.rcv_ssn << ","
+ << result->header.rcvinfo.rcv_assoc_id;
+ partial_message->size += result->size;
+ result.reset();
+ }
+
+ if (inmessage.msg_flags & MSG_EOR) {
+ // This is the last fragment, so we have something to return.
+ if (partial_message_iterator != partial_messages_.end()) {
+ // It was already merged into the message in the list, so now we pull
+ // that out of the list and return it.
+ CHECK(!result);
+ result = std::move(*partial_message_iterator);
+ partial_messages_.erase(partial_message_iterator);
+ VLOG(1) << "Final count: " << (result->partial_deliveries + 1)
+ << ", size: " << result->size
+ << ", for: " << result->header.rcvinfo.rcv_sid << ","
+ << result->header.rcvinfo.rcv_ssn << ","
+ << result->header.rcvinfo.rcv_assoc_id;
+ }
+ CHECK(result);
+ return result;
+ }
+ if (partial_message_iterator == partial_messages_.end()) {
+ VLOG(1) << "Starting fragment for: " << result->header.rcvinfo.rcv_sid
+ << "," << result->header.rcvinfo.rcv_ssn << ","
+ << result->header.rcvinfo.rcv_assoc_id;
+ // Need to record this as the first fragment.
+ partial_messages_.emplace_back(std::move(result));
+ }
}
- return result;
+}
+
+void SctpReadWrite::CloseSocket() {
+ if (fd_ == -1) {
+ return;
+ }
+ LOG(INFO) << "close(" << fd_ << ")";
+ PCHECK(close(fd_) == 0);
+ fd_ = -1;
+}
+
+void SctpReadWrite::DoSetMaxSize() {
+ // Have the kernel give us a factor of 10 more. This lets us have more than
+ // one full sized packet in flight.
+ size_t max_size = max_size_ * 10;
+
+ CHECK_GE(ReadRMemMax(), max_size)
+ << "rmem_max is too low. To increase rmem_max temporarily, do sysctl "
+ "-w net.core.rmem_max="
+ << max_size;
+ CHECK_GE(ReadWMemMax(), max_size)
+ << "wmem_max is too low. To increase wmem_max temporarily, do sysctl "
+ "-w net.core.wmem_max="
+ << max_size;
+ PCHECK(setsockopt(fd(), SOL_SOCKET, SO_RCVBUF, &max_size, sizeof(max_size)) ==
+ 0);
+ PCHECK(setsockopt(fd(), SOL_SOCKET, SO_SNDBUF, &max_size, sizeof(max_size)) ==
+ 0);
+}
+
+bool SctpReadWrite::ProcessNotification(const Message *message) {
+ const union sctp_notification *const snp =
+ reinterpret_cast<const union sctp_notification *>(message->data());
+ switch (snp->sn_header.sn_type) {
+ case SCTP_PARTIAL_DELIVERY_EVENT: {
+ const struct sctp_pdapi_event *const partial_delivery =
+ &snp->sn_pdapi_event;
+ CHECK_EQ(partial_delivery->pdapi_length, sizeof(*partial_delivery))
+ << ": Kernel's SCTP code is not a version we support";
+ switch (partial_delivery->pdapi_indication) {
+ case SCTP_PARTIAL_DELIVERY_ABORTED: {
+ const auto iterator = std::find_if(
+ partial_messages_.begin(), partial_messages_.end(),
+ [partial_delivery](const aos::unique_c_ptr<Message> &candidate) {
+ // TODO(Brian): Once we have new enough userpace headers, for
+ // kernels that support level-2 interleaving, we'll need to add
+ // this:
+ // candidate->header.rcvinfo.rcv_sid ==
+ // partial_delivery->pdapi_stream &&
+ // candidate->header.rcvinfo.rcv_ssn ==
+ // partial_delivery->pdapi_seq &&
+ return candidate->header.rcvinfo.rcv_assoc_id ==
+ partial_delivery->pdapi_assoc_id;
+ });
+ CHECK(iterator != partial_messages_.end())
+ << ": Got out of sync with the kernel for "
+ << partial_delivery->pdapi_assoc_id;
+ VLOG(1) << "Pruning partial delivery for "
+ << iterator->get()->header.rcvinfo.rcv_sid << ","
+ << iterator->get()->header.rcvinfo.rcv_ssn << ","
+ << iterator->get()->header.rcvinfo.rcv_assoc_id;
+ partial_messages_.erase(iterator);
+ }
+ return true;
+ }
+ } break;
+ }
+ return false;
}
void Message::LogRcvInfo() const {
diff --git a/aos/network/sctp_lib.h b/aos/network/sctp_lib.h
index 265d1f5..6b40600 100644
--- a/aos/network/sctp_lib.h
+++ b/aos/network/sctp_lib.h
@@ -5,8 +5,10 @@
#include <netinet/sctp.h>
#include <memory>
+#include <optional>
#include <string>
#include <string_view>
+#include <vector>
#include "aos/unique_malloc_ptr.h"
#include "gflags/gflags.h"
@@ -71,8 +73,54 @@
// Gets and logs the contents of the sctp_status message.
void LogSctpStatus(int fd, sctp_assoc_t assoc_id);
-// Read and allocate a message.
-aos::unique_c_ptr<Message> ReadSctpMessage(int fd, size_t max_size);
+// Manages reading and writing SCTP messages.
+class SctpReadWrite {
+ public:
+ SctpReadWrite() = default;
+ ~SctpReadWrite() { CloseSocket(); }
+
+ // Opens a new socket.
+ void OpenSocket(const struct sockaddr_storage &sockaddr_local);
+
+ // Sends a message to the kernel.
+ // Returns true for success. Will not send a partial message on failure.
+ bool SendMessage(int stream, std::string_view data, int time_to_live,
+ std::optional<struct sockaddr_storage> sockaddr_remote,
+ sctp_assoc_t snd_assoc_id);
+
+ // Reads from the kernel until a complete message is received or it blocks.
+ // Returns nullptr if the kernel blocks before returning a complete message.
+ aos::unique_c_ptr<Message> ReadMessage();
+
+ int fd() const { return fd_; }
+
+ void SetMaxSize(size_t max_size) {
+ CHECK(partial_messages_.empty())
+ << ": May not update size with queued fragments because we do not "
+ "track individual message sizes";
+ max_size_ = max_size;
+ if (fd_ != -1) {
+ DoSetMaxSize();
+ }
+ }
+
+ private:
+ void CloseSocket();
+ void DoSetMaxSize();
+
+ // Examines a notification message for ones we handle here.
+ // Returns true if the notification was handled by this class.
+ bool ProcessNotification(const Message *message);
+
+ int fd_ = -1;
+
+ // We use this as a unique identifier that just increments for each message.
+ uint32_t send_ppid_ = 0;
+
+ size_t max_size_ = 1000;
+
+ std::vector<aos::unique_c_ptr<Message>> partial_messages_;
+};
// Returns the max network buffer available for reading for a socket.
size_t ReadRMemMax();
diff --git a/aos/network/sctp_server.cc b/aos/network/sctp_server.cc
index 47a5719..894a1f1 100644
--- a/aos/network/sctp_server.cc
+++ b/aos/network/sctp_server.cc
@@ -23,72 +23,41 @@
SctpServer::SctpServer(std::string_view local_host, int local_port)
: sockaddr_local_(ResolveSocket(local_host, local_port)) {
while (true) {
- fd_ = socket(sockaddr_local_.ss_family, SOCK_SEQPACKET, IPPROTO_SCTP);
- LOG(INFO) << "socket(" << Family(sockaddr_local_)
- << ", SOCK_SEQPACKET, IPPROTOSCTP) = " << fd_;
- PCHECK(fd_ != -1);
+ sctp_.OpenSocket(sockaddr_local_);
{
- struct sctp_event_subscribe subscribe;
- memset(&subscribe, 0, sizeof(subscribe));
- subscribe.sctp_association_event = 1;
- subscribe.sctp_send_failure_event = 1;
- subscribe.sctp_partial_delivery_event = 1;
-
- PCHECK(setsockopt(fd_, SOL_SCTP, SCTP_EVENTS, (char *)&subscribe,
- sizeof(subscribe)) == 0);
- }
- {
- // Enable recvinfo when a packet arrives.
- int on = 1;
- PCHECK(setsockopt(fd_, IPPROTO_SCTP, SCTP_RECVRCVINFO, &on,
- sizeof(int)) == 0);
- }
- {
- // Per https://tools.ietf.org/html/rfc6458
- // Setting this to !0 allows event notifications to be interleaved
- // with data if enabled, and would have to be handled in the code.
- // Enabling interleaving would only matter during congestion, which
- // typically only happens during application startup.
- int interleaving = 0;
- PCHECK(setsockopt(fd_, IPPROTO_SCTP, SCTP_FRAGMENT_INTERLEAVE,
- &interleaving, sizeof(interleaving)) == 0);
- }
- {
// Turn off the NAGLE algorithm.
int on = 1;
- PCHECK(setsockopt(fd_, IPPROTO_SCTP, SCTP_NODELAY, &on, sizeof(int)) ==
+ PCHECK(setsockopt(fd(), IPPROTO_SCTP, SCTP_NODELAY, &on, sizeof(int)) ==
0);
}
{
int on = 1;
- PCHECK(setsockopt(fd_, SOL_SOCKET, SO_REUSEADDR, &on, sizeof(int)) == 0);
+ LOG(INFO) << "setsockopt(" << fd()
+ << ", SOL_SOCKET, SO_REUSEADDR, &on, sizeof(int)";
+ PCHECK(setsockopt(fd(), SOL_SOCKET, SO_REUSEADDR, &on, sizeof(int)) == 0);
}
// And go!
- if (bind(fd_, (struct sockaddr *)&sockaddr_local_,
+ if (bind(fd(), (struct sockaddr *)&sockaddr_local_,
sockaddr_local_.ss_family == AF_INET6
? sizeof(struct sockaddr_in6)
: sizeof(struct sockaddr_in)) != 0) {
PLOG(ERROR) << "Failed to bind, retrying";
- close(fd_);
+ close(fd());
std::this_thread::sleep_for(std::chrono::seconds(5));
continue;
}
- LOG(INFO) << "bind(" << fd_ << ", " << Address(sockaddr_local_) << ")";
+ LOG(INFO) << "bind(" << fd() << ", " << Address(sockaddr_local_) << ")";
- PCHECK(listen(fd_, 100) == 0);
+ PCHECK(listen(fd(), 100) == 0);
SetMaxSize(1000);
break;
}
}
-aos::unique_c_ptr<Message> SctpServer::Read() {
- return ReadSctpMessage(fd_, max_size_);
-}
-
bool SctpServer::Abort(sctp_assoc_t snd_assoc_id) {
// Use the assoc_id for the destination instead of the msg_name.
struct msghdr outmsg;
@@ -109,13 +78,12 @@
struct sctp_sndrcvinfo *sinfo = (struct sctp_sndrcvinfo *)CMSG_DATA(cmsg);
memset(sinfo, 0, sizeof(struct sctp_sndrcvinfo));
- sinfo->sinfo_ppid = ++ppid_;
sinfo->sinfo_stream = 0;
sinfo->sinfo_flags = SCTP_ABORT;
sinfo->sinfo_assoc_id = snd_assoc_id;
// And send.
- const ssize_t size = sendmsg(fd_, &outmsg, MSG_NOSIGNAL | MSG_DONTWAIT);
+ const ssize_t size = sendmsg(fd(), &outmsg, MSG_NOSIGNAL | MSG_DONTWAIT);
if (size == -1) {
if (errno == EPIPE || errno == EAGAIN || errno == ESHUTDOWN) {
return false;
@@ -127,53 +95,6 @@
}
}
-bool SctpServer::Send(std::string_view data, sctp_assoc_t snd_assoc_id,
- int stream, int timetolive) {
- struct iovec iov;
- iov.iov_base = const_cast<char *>(data.data());
- iov.iov_len = data.size();
-
- // Use the assoc_id for the destination instead of the msg_name.
- struct msghdr outmsg;
- outmsg.msg_namelen = 0;
-
- // Data to send.
- outmsg.msg_iov = &iov;
- outmsg.msg_iovlen = 1;
-
- // Build up the sndinfo message.
- char outcmsg[CMSG_SPACE(sizeof(struct sctp_sndrcvinfo))];
- outmsg.msg_control = outcmsg;
- outmsg.msg_controllen = CMSG_SPACE(sizeof(struct sctp_sndrcvinfo));
- outmsg.msg_flags = 0;
-
- struct cmsghdr *cmsg = CMSG_FIRSTHDR(&outmsg);
- cmsg->cmsg_level = IPPROTO_SCTP;
- cmsg->cmsg_type = SCTP_SNDRCV;
- cmsg->cmsg_len = CMSG_LEN(sizeof(struct sctp_sndrcvinfo));
-
- struct sctp_sndrcvinfo *sinfo = (struct sctp_sndrcvinfo *)CMSG_DATA(cmsg);
- memset(sinfo, 0, sizeof(struct sctp_sndrcvinfo));
- sinfo->sinfo_ppid = ++ppid_;
- sinfo->sinfo_stream = stream;
- sinfo->sinfo_flags = 0;
- sinfo->sinfo_assoc_id = snd_assoc_id;
- sinfo->sinfo_timetolive = timetolive;
-
- // And send.
- const ssize_t size = sendmsg(fd_, &outmsg, MSG_NOSIGNAL | MSG_DONTWAIT);
- if (size == -1) {
- if (errno == EPIPE || errno == EAGAIN || errno == ESHUTDOWN) {
- return false;
- }
- PCHECK(size == static_cast<ssize_t>(data.size()));
- return false;
- } else {
- CHECK_EQ(static_cast<ssize_t>(data.size()), size);
- return true;
- }
-}
-
void SctpServer::SetPriorityScheduler(sctp_assoc_t assoc_id) {
struct sctp_assoc_value scheduler;
memset(&scheduler, 0, sizeof(scheduler));
diff --git a/aos/network/sctp_server.h b/aos/network/sctp_server.h
index d995c55..c6737d6 100644
--- a/aos/network/sctp_server.h
+++ b/aos/network/sctp_server.h
@@ -24,23 +24,23 @@
public:
SctpServer(std::string_view local_host = "0.0.0.0", int local_port = 9971);
- ~SctpServer() {
- LOG(INFO) << "close(" << fd_ << ")";
- PCHECK(close(fd_) == 0);
- }
+ ~SctpServer() {}
// Receives the next packet from the remote.
- aos::unique_c_ptr<Message> Read();
+ aos::unique_c_ptr<Message> Read() { return sctp_.ReadMessage(); }
// Sends a block of data to a client on a stream with a TTL. Returns true on
// success.
bool Send(std::string_view data, sctp_assoc_t snd_assoc_id, int stream,
- int timetolive);
+ int time_to_live) {
+ return sctp_.SendMessage(stream, data, time_to_live, std::nullopt,
+ snd_assoc_id);
+ }
// Aborts a connection. Returns true on success.
bool Abort(sctp_assoc_t snd_assoc_id);
- int fd() { return fd_; }
+ int fd() { return sctp_.fd(); }
// Enables the priority scheduler. This is a SCTP feature which lets us
// configure the priority per stream so that higher priority packets don't get
@@ -51,27 +51,11 @@
void SetStreamPriority(sctp_assoc_t assoc_id, int stream_id,
uint16_t priority);
- void SetMaxSize(size_t max_size) {
- max_size_ = max_size;
- // Have the kernel give us a factor of 10 more. This lets us have more than
- // one full sized packet in flight.
- max_size = max_size * 10;
-
- CHECK_GE(ReadRMemMax(), max_size);
- CHECK_GE(ReadWMemMax(), max_size);
- PCHECK(setsockopt(fd_, SOL_SOCKET, SO_RCVBUF, &max_size,
- sizeof(max_size)) == 0);
- PCHECK(setsockopt(fd_, SOL_SOCKET, SO_SNDBUF, &max_size,
- sizeof(max_size)) == 0);
- }
+ void SetMaxSize(size_t max_size) { sctp_.SetMaxSize(max_size); }
private:
struct sockaddr_storage sockaddr_local_;
- int fd_;
-
- size_t max_size_ = 1000;
-
- int ppid_ = 1;
+ SctpReadWrite sctp_;
};
} // namespace message_bridge
diff --git a/aos/realtime.cc b/aos/realtime.cc
index cd9ea43..dc53a37 100644
--- a/aos/realtime.cc
+++ b/aos/realtime.cc
@@ -36,6 +36,12 @@
int MallocHook_AddDeleteHook(MallocHook_DeleteHook hook) __attribute__((weak));
int MallocHook_RemoveDeleteHook(MallocHook_DeleteHook hook)
__attribute__((weak));
+
+// Declare tc_malloc weak so we can check if it exists.
+void *tc_malloc(size_t size) __attribute__((weak));
+
+void *__libc_malloc(size_t size);
+void __libc_free(void *ptr);
} // extern "C"
namespace FLAG__namespace_do_not_use_directly_use_DECLARE_double_instead {
@@ -251,17 +257,57 @@
}
}
+extern "C" {
+
+// malloc hooks for libc. Tcmalloc will replace everything it finds (malloc,
+// __libc_malloc, etc.), so we need its specific hook above as well.
+void *aos_malloc_hook(size_t size) {
+ if (FLAGS_die_on_malloc && aos::is_realtime) {
+ aos::is_realtime = false;
+ RAW_LOG(FATAL, "Malloced %zu bytes", size);
+ return nullptr;
+ } else {
+ return __libc_malloc(size);
+ }
+}
+
+void aos_free_hook(void *ptr) {
+ if (FLAGS_die_on_malloc && aos::is_realtime && ptr != nullptr) {
+ aos::is_realtime = false;
+ RAW_LOG(FATAL, "Deleted %p", ptr);
+ } else {
+ __libc_free(ptr);
+ }
+}
+
+void *malloc(size_t size) __attribute__((weak, alias("aos_malloc_hook")));
+void free(void *ptr) __attribute__((weak, alias("aos_free_hook")));
+
+}
+
void RegisterMallocHook() {
if (FLAGS_die_on_malloc) {
- if (&MallocHook_AddNewHook != nullptr) {
- CHECK(MallocHook_AddNewHook(&NewHook));
+ // tcmalloc redefines __libc_malloc, so use this as a feature test.
+ if (&__libc_malloc == &tc_malloc) {
+ RAW_LOG(INFO, "Hooking tcmalloc for die_on_malloc");
+ if (&MallocHook_AddNewHook != nullptr) {
+ CHECK(MallocHook_AddNewHook(&NewHook));
+ } else {
+ has_malloc_hook = false;
+ }
+ if (&MallocHook_AddDeleteHook != nullptr) {
+ CHECK(MallocHook_AddDeleteHook(&DeleteHook));
+ } else {
+ has_malloc_hook = false;
+ }
} else {
- has_malloc_hook = false;
- }
- if (&MallocHook_AddDeleteHook != nullptr) {
- CHECK(MallocHook_AddDeleteHook(&DeleteHook));
- } else {
- has_malloc_hook = false;
+ RAW_LOG(INFO, "Replacing glibc malloc");
+ if (&malloc != &aos_malloc_hook) {
+ has_malloc_hook = false;
+ }
+ if (&free != &aos_free_hook) {
+ has_malloc_hook = false;
+ }
}
}
}
diff --git a/frc971/control_loops/drivetrain/drivetrain_test_lib.cc b/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
index 7680688..3fa138c 100644
--- a/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
@@ -41,7 +41,7 @@
coefs.emplace_back(new StateFeedbackPlantCoefficients<4, 2, 2, double>(
dt_config.make_drivetrain_loop().plant().coefficients(ii)));
}
- return StateFeedbackPlant<4, 2, 2, double>(&coefs);
+ return StateFeedbackPlant<4, 2, 2, double>(std::move(coefs));
}
} // namespace
diff --git a/frc971/control_loops/hybrid_state_feedback_loop.h b/frc971/control_loops/hybrid_state_feedback_loop.h
index 6285024..23a3d21 100644
--- a/frc971/control_loops/hybrid_state_feedback_loop.h
+++ b/frc971/control_loops/hybrid_state_feedback_loop.h
@@ -63,8 +63,8 @@
StateFeedbackHybridPlant(
::std::vector<::std::unique_ptr<StateFeedbackHybridPlantCoefficients<
number_of_states, number_of_inputs, number_of_outputs>>>
- *coefficients)
- : coefficients_(::std::move(*coefficients)), index_(0) {
+ &&coefficients)
+ : coefficients_(::std::move(coefficients)), index_(0) {
Reset();
}
@@ -106,14 +106,14 @@
Scalar U_max(int i, int j) const { return U_max()(i, j); }
const Eigen::Matrix<Scalar, number_of_states, 1> &X() const { return X_; }
- Scalar X(int i, int j) const { return X()(i, j); }
+ Scalar X(int i) const { return X()(i); }
const Eigen::Matrix<Scalar, number_of_outputs, 1> &Y() const { return Y_; }
- Scalar Y(int i, int j) const { return Y()(i, j); }
+ Scalar Y(int i) const { return Y()(i); }
Eigen::Matrix<Scalar, number_of_states, 1> &mutable_X() { return X_; }
- Scalar &mutable_X(int i, int j) { return mutable_X()(i, j); }
+ Scalar &mutable_X(int i) { return mutable_X()(i); }
Eigen::Matrix<Scalar, number_of_outputs, 1> &mutable_Y() { return Y_; }
- Scalar &mutable_Y(int i, int j) { return mutable_Y()(i, j); }
+ Scalar &mutable_Y(int i) { return mutable_Y()(i); }
const StateFeedbackHybridPlantCoefficients<number_of_states, number_of_inputs,
number_of_outputs, Scalar>
@@ -247,8 +247,12 @@
explicit HybridKalman(
::std::vector<::std::unique_ptr<HybridKalmanCoefficients<
number_of_states, number_of_inputs, number_of_outputs, Scalar>>>
- *observers)
- : coefficients_(::std::move(*observers)) {}
+ &&observers)
+ : coefficients_(::std::move(observers)) {
+ R_.setZero();
+ X_hat_.setZero();
+ P_ = coefficients().P_steady_state;
+ }
HybridKalman(HybridKalman &&other) : index_(other.index_) {
::std::swap(coefficients_, other.coefficients_);
@@ -281,22 +285,37 @@
return X_hat_;
}
Eigen::Matrix<Scalar, number_of_states, 1> &mutable_X_hat() { return X_hat_; }
+ Scalar &mutable_X_hat(int i) { return mutable_X_hat()(i); }
+ Scalar X_hat(int i) const { return X_hat_(i); }
void Reset(StateFeedbackHybridPlant<number_of_states, number_of_inputs,
number_of_outputs> *plant) {
X_hat_.setZero();
P_ = coefficients().P_steady_state;
- UpdateQR(plant, ::std::chrono::milliseconds(5));
+ UpdateQR(plant, coefficients().Q_continuous, coefficients().R_continuous,
+ ::std::chrono::milliseconds(5));
}
void Predict(StateFeedbackHybridPlant<number_of_states, number_of_inputs,
number_of_outputs, Scalar> *plant,
const Eigen::Matrix<Scalar, number_of_inputs, 1> &new_u,
::std::chrono::nanoseconds dt) {
+ Predict(plant, new_u, dt, coefficients().Q_continuous,
+ coefficients().R_continuous);
+ }
+
+ void Predict(
+ StateFeedbackHybridPlant<number_of_states, number_of_inputs,
+ number_of_outputs, Scalar> *plant,
+ const Eigen::Matrix<Scalar, number_of_inputs, 1> &new_u,
+ ::std::chrono::nanoseconds dt,
+ Eigen::Matrix<Scalar, number_of_states, number_of_states> Q_continuous,
+ Eigen::Matrix<Scalar, number_of_outputs, number_of_outputs>
+ R_continuous) {
// Trigger the predict step. This will update A() and B() in the plant.
mutable_X_hat() = plant->Update(X_hat(), new_u, dt);
- UpdateQR(plant, dt);
+ UpdateQR(plant, Q_continuous, R_continuous, dt);
P_ = plant->A() * P_ * plant->A().transpose() + Q_;
}
@@ -305,18 +324,29 @@
number_of_outputs, Scalar> &plant,
const Eigen::Matrix<Scalar, number_of_inputs, 1> &U,
const Eigen::Matrix<Scalar, number_of_outputs, 1> &Y) {
+ DynamicCorrect(plant.C(), plant.D(), U, Y, R_);
+ }
+
+ // Corrects based on the sensor information available.
+ template <int number_of_measurements>
+ void DynamicCorrect(
+ const Eigen::Matrix<Scalar, number_of_measurements, number_of_states> &C,
+ const Eigen::Matrix<Scalar, number_of_measurements, number_of_inputs> &D,
+ const Eigen::Matrix<Scalar, number_of_inputs, 1> &U,
+ const Eigen::Matrix<Scalar, number_of_outputs, 1> &Y,
+ const Eigen::Matrix<Scalar, number_of_outputs, number_of_outputs> &R) {
Eigen::Matrix<Scalar, number_of_outputs, 1> Y_bar =
- Y - (plant.C() * X_hat_ + plant.D() * U);
+ Y - (C * X_hat_ + D * U);
Eigen::Matrix<Scalar, number_of_outputs, number_of_outputs> S =
- plant.C() * P_ * plant.C().transpose() + R_;
+ C * P_ * C.transpose() + R;
Eigen::Matrix<Scalar, number_of_states, number_of_outputs> KalmanGain;
- KalmanGain =
- (S.transpose().ldlt().solve((P() * plant.C().transpose()).transpose()))
- .transpose();
+ KalmanGain = (S.transpose().ldlt().solve((P() * C.transpose()).transpose()))
+ .transpose();
X_hat_ = X_hat_ + KalmanGain * Y_bar;
- P_ = (plant.coefficients().A_continuous.Identity() -
- KalmanGain * plant.C()) *
- P();
+ P_ =
+ (Eigen::Matrix<Scalar, number_of_states, number_of_states>::Identity() -
+ KalmanGain * C) *
+ P();
}
// Sets the current controller to be index, clamped to be within range.
@@ -345,17 +375,17 @@
}
private:
- void UpdateQR(StateFeedbackHybridPlant<number_of_states, number_of_inputs,
- number_of_outputs> *plant,
- ::std::chrono::nanoseconds dt) {
- ::frc971::controls::DiscretizeQ(coefficients().Q_continuous,
- plant->coefficients().A_continuous, dt,
- &Q_);
+ void UpdateQR(
+ StateFeedbackHybridPlant<number_of_states, number_of_inputs,
+ number_of_outputs> *plant,
+ Eigen::Matrix<Scalar, number_of_states, number_of_states> Q_continuous,
+ Eigen::Matrix<Scalar, number_of_outputs, number_of_outputs> R_continuous,
+ ::std::chrono::nanoseconds dt) {
+ frc971::controls::DiscretizeQ(Q_continuous,
+ plant->coefficients().A_continuous, dt, &Q_);
Eigen::Matrix<Scalar, number_of_outputs, number_of_outputs> Rtemp =
- (coefficients().R_continuous +
- coefficients().R_continuous.transpose()) /
- static_cast<Scalar>(2.0);
+ (R_continuous + R_continuous.transpose()) / static_cast<Scalar>(2.0);
R_ = Rtemp / ::aos::time::TypedDurationInSeconds<Scalar>(dt);
}
diff --git a/frc971/control_loops/hybrid_state_feedback_loop_test.cc b/frc971/control_loops/hybrid_state_feedback_loop_test.cc
index 13f74fa..c5b0771 100644
--- a/frc971/control_loops/hybrid_state_feedback_loop_test.cc
+++ b/frc971/control_loops/hybrid_state_feedback_loop_test.cc
@@ -84,7 +84,7 @@
plants[0] = ::std::unique_ptr<StateFeedbackHybridPlantCoefficients<3, 1, 1>>(
new StateFeedbackHybridPlantCoefficients<3, 1, 1>(
MakeIntegralShooterPlantCoefficients()));
- return StateFeedbackHybridPlant<3, 1, 1>(&plants);
+ return StateFeedbackHybridPlant<3, 1, 1>(std::move(plants));
}
StateFeedbackController<3, 1, 1> MakeIntegralShooterController() {
@@ -94,7 +94,7 @@
::std::unique_ptr<StateFeedbackControllerCoefficients<3, 1, 1>>(
new StateFeedbackControllerCoefficients<3, 1, 1>(
MakeIntegralShooterControllerCoefficients()));
- return StateFeedbackController<3, 1, 1>(&controllers);
+ return StateFeedbackController<3, 1, 1>(std::move(controllers));
}
HybridKalman<3, 1, 1> MakeIntegralShooterObserver() {
@@ -103,7 +103,7 @@
observers[0] = ::std::unique_ptr<HybridKalmanCoefficients<3, 1, 1>>(
new HybridKalmanCoefficients<3, 1, 1>(
MakeIntegralShooterObserverCoefficients()));
- return HybridKalman<3, 1, 1>(&observers);
+ return HybridKalman<3, 1, 1>(std::move(observers));
}
StateFeedbackLoop<3, 1, 1, double, StateFeedbackHybridPlant<3, 1, 1>,
@@ -134,7 +134,7 @@
v_plant;
v_plant.emplace_back(
new StateFeedbackPlantCoefficients<2, 4, 7>(coefficients));
- StateFeedbackPlant<2, 4, 7> plant(&v_plant);
+ StateFeedbackPlant<2, 4, 7> plant(std::move(v_plant));
plant.Update(Eigen::Matrix<double, 4, 1>::Zero());
plant.Reset();
plant.CheckU(Eigen::Matrix<double, 4, 1>::Zero());
@@ -145,7 +145,7 @@
v_controller.emplace_back(new StateFeedbackControllerCoefficients<2, 4, 7>(
Eigen::Matrix<double, 4, 2>::Identity(),
Eigen::Matrix<double, 4, 2>::Identity()));
- StateFeedbackController<2, 4, 7> controller(&v_controller);
+ StateFeedbackController<2, 4, 7> controller(std::move(v_controller));
::std::vector<::std::unique_ptr<StateFeedbackObserverCoefficients<2, 4, 7>>>
v_observer;
@@ -153,7 +153,7 @@
Eigen::Matrix<double, 2, 7>::Identity(),
Eigen::Matrix<double, 2, 2>::Identity(),
Eigen::Matrix<double, 7, 7>::Identity()));
- StateFeedbackObserver<2, 4, 7> observer(&v_observer);
+ StateFeedbackObserver<2, 4, 7> observer(std::move(v_observer));
StateFeedbackLoop<2, 4, 7> test_loop(
::std::move(plant), ::std::move(controller), ::std::move(observer));
diff --git a/frc971/control_loops/python/control_loop.py b/frc971/control_loops/python/control_loop.py
index 74141c1..cea1fdd 100644
--- a/frc971/control_loops/python/control_loop.py
+++ b/frc971/control_loops/python/control_loop.py
@@ -216,7 +216,7 @@
fd.write(' plants[%d] = ::std::unique_ptr<%s>(new %s(%s));\n'
% (index, self._PlantCoeffType(),
self._PlantCoeffType(), loop.PlantFunction()))
- fd.write(' return %s(&plants);\n' % self._PlantType())
+ fd.write(' return %s(std::move(plants));\n' % self._PlantType())
fd.write('}\n\n')
fd.write('%s Make%sController() {\n' % (self._ControllerType(),
@@ -229,7 +229,8 @@
' controllers[%d] = ::std::unique_ptr<%s>(new %s(%s));\n'
% (index, self._ControllerCoeffType(),
self._ControllerCoeffType(), loop.ControllerFunction()))
- fd.write(' return %s(&controllers);\n' % self._ControllerType())
+ fd.write(' return %s(std::move(controllers));\n' %
+ self._ControllerType())
fd.write('}\n\n')
fd.write('%s Make%sObserver() {\n' % (self._ObserverType(),
@@ -241,7 +242,8 @@
' observers[%d] = ::std::unique_ptr<%s>(new %s(%s));\n'
% (index, self._ObserverCoeffType(),
self._ObserverCoeffType(), loop.ObserverFunction()))
- fd.write(' return %s(&observers);\n' % self._ObserverType())
+ fd.write(
+ ' return %s(std::move(observers));\n' % self._ObserverType())
fd.write('}\n\n')
fd.write('%s Make%sLoop() {\n' % (self._LoopType(),
diff --git a/frc971/control_loops/python/drawing_constants.py b/frc971/control_loops/python/drawing_constants.py
index 8d64e0b..644e449 100644
--- a/frc971/control_loops/python/drawing_constants.py
+++ b/frc971/control_loops/python/drawing_constants.py
@@ -1,6 +1,5 @@
import cairo
from color import Color, palette
-from constants import *
import numpy as np
@@ -59,467 +58,6 @@
cr.show_text(text)
cr.scale(widthb, -heightb)
-
-def draw_at_home_grid(cr):
- field = np.zeros(shape=(5, 11), dtype=bool)
- # field[row from bottom][column from left]
-
- if GALACTIC_SEARCH in FIELD.tags:
- # Galactic search start zone
- field[1][0] = True
- field[3][0] = True
-
- # Galactic search end zone
- field[1][10] = True
- field[3][10] = True
-
- if ARED in FIELD.tags:
- field[4][5] = True
- field[2][2] = True
- field[1][4] = True
- elif ABLUE in FIELD.tags:
- field[0][5] = True
- field[3][6] = True
- field[2][8] = True
- elif BRED in FIELD.tags:
- field[3][2] = True
- field[1][4] = True
- field[3][6] = True
- elif BBLUE in FIELD.tags:
- field[1][5] = True
- field[3][7] = True
- field[1][9] = True
- elif AUTONAV in FIELD.tags:
- # start/end zone
- field[1][0] = True
- field[1][1] = True
- field[3][0] = True
- field[3][1] = True
-
- if BARREL in FIELD.tags:
- # barrels
- field[1][4] = True
- field[3][7] = True
- field[1][9] = True
- if SLALOM in FIELD.tags:
- field[1][3:8] = True # 3 to 7 inclusive
- field[1][9] = True
- if BOUNCE in FIELD.tags:
- # turn on two rows
- field[1][:11] = True
- field[3][:11] = True
-
- # turn off parts of rows
- field[3][2] = False
- field[3][5] = False
- field[3][8] = False
-
- field[1][3] = False
- field[1][5] = False
- field[1][8] = False
-
- # markers to hit
- field[4][2] = True
- field[4][5] = True
- field[4][8] = True
-
- # Move origin to bottom left
- xorigin = -mToPx(FIELD.width) / 2.0
- yorigin = -mToPx(FIELD.length) / 2.0
-
- color = palette["BLACK"]
- # markers are at least 6.35 x 6.35 cm
- marker_length = mToPx(0.0635)
-
- for row, row_array in enumerate(field):
- for column, has_marker in enumerate(row_array):
- one_indexed_row = row + 1
- one_indexed_column = column + 1
-
- # 76.2 cm increments
- pos_y = one_indexed_row * mToPx(0.762)
- pos_x = one_indexed_column * mToPx(0.762)
-
- if has_marker:
- draw_px_x(cr, xorigin + pos_x, yorigin + pos_y, marker_length,
- color)
-
-
-def markers(cr):
- SHOW_MARKERS = False
- if SHOW_MARKERS:
- # Shield Generator Reference
- color = palette["BLUE"]
- yorigin = 0 - SCREEN_SIZE / 2 # Move origin to bottom left
- # Top Left
- draw_circle(cr, mToPx(inToM(206.625)),
- yorigin + mToPx(inToM(212.097), True), 10, color)
- # Top Right
- draw_circle(cr, mToPx(inToM(373.616)),
- yorigin + mToPx(inToM(281.26), True), 10, color)
- # Bottom Right
- draw_circle(cr, mToPx(inToM(422.625)),
- yorigin + mToPx(inToM(124.67), True), 10, color)
- # Bottom Left
- draw_circle(cr, mToPx(inToM(255.634)),
- yorigin + mToPx(inToM(55.5), True), 10, color)
-
- # Trench Run Reference
- color = palette["GREEN"]
- # Bottom Trench Run
- # Bottom Right
- draw_circle(cr, mToPx(inToM(206.625)), yorigin, 10, color)
- # Top Right
- draw_circle(cr, mToPx(inToM(206.625)),
- yorigin + mToPx(inToM(55.5), True), 10, color)
- # Top Left
- draw_circle(cr, mToPx(inToM(422.625)),
- yorigin + mToPx(inToM(55.5), True), 10, color)
- # Bottom Left
- draw_circle(cr, mToPx(inToM(422.625)), yorigin, 10, color)
-
- # Top Trench Run
- # Top Right
- draw_circle(cr, mToPx(inToM(206.625)),
- yorigin + mToPx(inToM(323.25), True), 10, color)
- # Bottom Right
- draw_circle(cr, mToPx(inToM(206.625)),
- yorigin + mToPx(inToM(281.26), True), 10, color)
- # Top Left
- draw_circle(cr, mToPx(inToM(422.625)),
- yorigin + mToPx(inToM(281.26), True), 10, color)
- # Bottom Left
- draw_circle(cr, mToPx(inToM(422.625)),
- yorigin + mToPx(inToM(323.25), True), 10, color)
- cr.stroke()
-
-
-def draw_init_lines(cr):
- set_color(cr, palette["RED"])
- init_line_x = FIELD.width / 2.0 - inToM(120)
- init_start_y = -FIELD.length / 2.0
- init_end_y = FIELD.length / 2.0
- cr.move_to(mToPx(init_line_x), mToPx(init_start_y))
- cr.line_to(mToPx(init_line_x), mToPx(init_end_y))
-
- cr.move_to(mToPx(-init_line_x), mToPx(init_start_y))
- cr.line_to(mToPx(-init_line_x), mToPx(init_end_y))
-
- cr.stroke()
-
-
-def draw_trench_run(cr):
- edge_of_field_y = FIELD.length / 2.0
- edge_of_trench_y = edge_of_field_y - inToM(55.5)
- trench_start_x = inToM(-108.0)
- trench_length_x = inToM(216.0)
- ball_line_y = edge_of_field_y - inToM(27.75)
- ball_one_x = -inToM(72)
- ball_two_x = -inToM(36)
- ball_three_x = 0.0
- # The fourth/fifth balls are referenced off of the init line...
- ball_fourfive_x = FIELD.width / 2.0 - inToM(120.0 + 130.36)
-
- for sign in [1.0, -1.0]:
- set_color(cr, palette["GREEN"])
- cr.rectangle(
- mToPx(sign * trench_start_x), mToPx(sign * edge_of_field_y),
- mToPx(sign * trench_length_x),
- mToPx(sign * (edge_of_trench_y - edge_of_field_y)))
- cr.stroke()
- draw_circle(cr, mToPx(sign * ball_one_x), mToPx(sign * ball_line_y),
- mToPx(0.1), palette["YELLOW"])
- draw_circle(cr, mToPx(sign * ball_two_x), mToPx(sign * ball_line_y),
- mToPx(0.1), palette["YELLOW"])
- draw_circle(cr, mToPx(sign * ball_three_x), mToPx(sign * ball_line_y),
- mToPx(0.1), palette["YELLOW"])
-
- cr.stroke()
-
-
-def draw_shield_generator(cr):
- set_color(cr, palette["BLUE"])
- cr.save()
- cr.rotate(22.5 * np.pi / 180.)
- generator_width = mToPx(inToM(14 * 12 + 0.75))
- generator_height = mToPx(inToM(13 * 12 + 1.5))
- cr.rectangle(-generator_width / 2.0, -generator_height / 2.0,
- generator_width, generator_height)
- cr.restore()
-
- cr.stroke()
-
-
-def draw_control_panel(cr): # Base plates are not included
- set_color(cr, palette["LIGHT_GREY"])
- edge_of_field_y = FIELD.length / 2.0
- edge_of_trench_y = edge_of_field_y - inToM(55.5)
- high_x = inToM(374.59) - FIELD.width / 2.0
- low_x = high_x - inToM(30)
- for sign in [1.0, -1.0]:
- # Bottom Control Panel
- # Top Line
- cr.rectangle(sign * mToPx(high_x), sign * mToPx(edge_of_field_y),
- -sign * mToPx(inToM(30)), -sign * mToPx(inToM(55.5)))
-
- cr.stroke()
-
-
-def draw_HAB(cr):
- # BASE Constants
- X_BASE = 0
- Y_BASE = 0
- R = 0.381 - .1
- BACKWALL_X = X_BASE
- LOADING_Y = mToPx(4.129151) - mToPx(0.696976)
- # HAB Levels 2 and 3 called in variables backhab
- # draw loading stations
- cr.move_to(0, LOADING_Y)
- cr.line_to(mToPx(0.6), LOADING_Y)
- cr.move_to(mToPx(R), LOADING_Y)
- cr.arc(mToPx(R), LOADING_Y, 5, 0, np.pi * 2.0)
- cr.move_to(0, -1.0 * LOADING_Y)
- cr.line_to(mToPx(0.6), -1.0 * LOADING_Y)
- cr.move_to(mToPx(R), -1.0 * LOADING_Y)
- cr.arc(mToPx(R), -1.0 * LOADING_Y, 5, 0, np.pi * 2.0)
-
- # HAB Levels 2 and 3 called in variables backhab
- WIDTH_BACKHAB = mToPx(1.2192)
-
- Y_TOP_BACKHAB_BOX = Y_BASE + mToPx(0.6096)
- BACKHAB_LV2_LENGTH = mToPx(1.016)
-
- BACKHAB_LV3_LENGTH = mToPx(1.2192)
- Y_LV3_BOX = Y_TOP_BACKHAB_BOX - BACKHAB_LV3_LENGTH
-
- Y_BOTTOM_BACKHAB_BOX = Y_LV3_BOX - BACKHAB_LV2_LENGTH
-
- # HAB LEVEL 1
- X_LV1_BOX = BACKWALL_X + WIDTH_BACKHAB
-
- WIDTH_LV1_BOX = mToPx(0.90805)
- LENGTH_LV1_BOX = mToPx(1.6256)
-
- Y_BOTTOM_LV1_BOX = Y_BASE - LENGTH_LV1_BOX
-
- # Ramp off Level 1
- X_RAMP = X_LV1_BOX
-
- Y_TOP_RAMP = Y_BASE + LENGTH_LV1_BOX
- WIDTH_TOP_RAMP = mToPx(1.20015)
- LENGTH_TOP_RAMP = Y_BASE + mToPx(0.28306)
-
- X_MIDDLE_RAMP = X_RAMP + WIDTH_LV1_BOX
- Y_MIDDLE_RAMP = Y_BOTTOM_LV1_BOX
- LENGTH_MIDDLE_RAMP = 2 * LENGTH_LV1_BOX
- WIDTH_MIDDLE_RAMP = WIDTH_TOP_RAMP - WIDTH_LV1_BOX
-
- Y_BOTTOM_RAMP = Y_BASE - LENGTH_LV1_BOX - LENGTH_TOP_RAMP
-
- # Side Bars to Hold in balls
- X_BARS = BACKWALL_X
- WIDTH_BARS = WIDTH_BACKHAB
- LENGTH_BARS = mToPx(0.574675)
-
- Y_TOP_BAR = Y_TOP_BACKHAB_BOX + BACKHAB_LV2_LENGTH
-
- Y_BOTTOM_BAR = Y_BOTTOM_BACKHAB_BOX - LENGTH_BARS
-
- set_color(cr, palette["BLACK"])
- cr.rectangle(BACKWALL_X, Y_TOP_BACKHAB_BOX, WIDTH_BACKHAB,
- BACKHAB_LV2_LENGTH)
- cr.rectangle(BACKWALL_X, Y_LV3_BOX, WIDTH_BACKHAB, BACKHAB_LV3_LENGTH)
- cr.rectangle(BACKWALL_X, Y_BOTTOM_BACKHAB_BOX, WIDTH_BACKHAB,
- BACKHAB_LV2_LENGTH)
- cr.rectangle(X_LV1_BOX, Y_BASE, WIDTH_LV1_BOX, LENGTH_LV1_BOX)
- cr.rectangle(X_LV1_BOX, Y_BOTTOM_LV1_BOX, WIDTH_LV1_BOX, LENGTH_LV1_BOX)
- cr.rectangle(X_RAMP, Y_TOP_RAMP, WIDTH_TOP_RAMP, LENGTH_TOP_RAMP)
- cr.rectangle(X_MIDDLE_RAMP, Y_MIDDLE_RAMP, WIDTH_MIDDLE_RAMP,
- LENGTH_MIDDLE_RAMP)
- cr.rectangle(X_RAMP, Y_BOTTOM_RAMP, WIDTH_TOP_RAMP, LENGTH_TOP_RAMP)
- cr.rectangle(X_BARS, Y_TOP_BAR, WIDTH_BARS, LENGTH_BARS)
- cr.rectangle(X_BARS, Y_BOTTOM_BAR, WIDTH_BARS, LENGTH_BARS)
- cr.stroke()
-
- cr.set_line_join(cairo.LINE_JOIN_ROUND)
-
- cr.stroke()
-
- #draw 0, 0
- set_color(cr, palette["BLACK"])
- cr.move_to(0, 0)
- cr.line_to(0, 0 + mToPx(8.2296 / 2.0))
- cr.move_to(0, 0)
- cr.line_to(0, 0 + mToPx(-8.2296 / 2.0))
- cr.move_to(0, 0)
- cr.line_to(0 + mToPx(8.2296), 0)
-
- cr.stroke()
-
-
-def draw_rockets(cr):
- # BASE Constants
- X_BASE = mToPx(2.41568)
- Y_BASE = 0
- # Robot longitudinal radius
- R = 0.381
- near_side_rocket_center = [
- X_BASE + mToPx((2.89973 + 3.15642) / 2.0), Y_BASE + mToPx(
- (3.86305 + 3.39548) / 2.0)
- ]
- middle_rocket_center = [
- X_BASE + mToPx((3.15642 + 3.6347) / 2.0), Y_BASE + mToPx(
- (3.39548 + 3.392380) / 2.0)
- ]
- far_side_rocket_center = [
- X_BASE + mToPx((3.63473 + 3.89984) / 2.0), Y_BASE + mToPx(
- (3.39238 + 3.86305) / 2.0)
- ]
-
- cr.move_to(near_side_rocket_center[0], near_side_rocket_center[1])
- cr.line_to(near_side_rocket_center[0] - 0.8 * mToPx(0.866),
- near_side_rocket_center[1] - 0.8 * mToPx(0.5))
- cr.move_to(near_side_rocket_center[0] - R * mToPx(0.866),
- near_side_rocket_center[1] - R * mToPx(0.5))
- cr.arc(near_side_rocket_center[0] - R * mToPx(0.866),
- near_side_rocket_center[1] - R * mToPx(0.5), 5, 0, np.pi * 2.0)
-
- cr.move_to(middle_rocket_center[0], middle_rocket_center[1])
- cr.line_to(middle_rocket_center[0], middle_rocket_center[1] - mToPx(0.8))
- cr.move_to(middle_rocket_center[0], middle_rocket_center[1] - mToPx(R))
- cr.arc(middle_rocket_center[0], middle_rocket_center[1] - mToPx(R), 5, 0,
- np.pi * 2.0)
-
- cr.move_to(far_side_rocket_center[0], far_side_rocket_center[1])
- cr.line_to(far_side_rocket_center[0] + 0.8 * mToPx(0.866),
- far_side_rocket_center[1] - 0.8 * mToPx(0.5))
- cr.move_to(far_side_rocket_center[0] + R * mToPx(0.866),
- far_side_rocket_center[1] - R * mToPx(0.5))
- cr.arc(far_side_rocket_center[0] + R * mToPx(0.866),
- far_side_rocket_center[1] - R * mToPx(0.5), 5, 0, np.pi * 2.0)
-
- #print(far_side_rocket_center)
- near_side_rocket_center = [
- X_BASE + mToPx((2.89973 + 3.15642) / 2.0), Y_BASE - mToPx(
- (3.86305 + 3.39548) / 2.0)
- ]
- middle_rocket_center = [
- X_BASE + mToPx((3.15642 + 3.6347) / 2.0), Y_BASE - mToPx(
- (3.39548 + 3.392380) / 2.0)
- ]
- far_side_rocket_center = [
- X_BASE + mToPx((3.63473 + 3.89984) / 2.0), Y_BASE - mToPx(
- (3.39238 + 3.86305) / 2.0)
- ]
-
- cr.move_to(near_side_rocket_center[0], near_side_rocket_center[1])
- cr.line_to(near_side_rocket_center[0] - 0.8 * mToPx(0.866),
- near_side_rocket_center[1] + 0.8 * mToPx(0.5))
-
- cr.move_to(middle_rocket_center[0], middle_rocket_center[1])
- cr.line_to(middle_rocket_center[0], middle_rocket_center[1] + mToPx(0.8))
-
- cr.move_to(far_side_rocket_center[0], far_side_rocket_center[1])
- cr.line_to(far_side_rocket_center[0] + 0.8 * mToPx(0.866),
- far_side_rocket_center[1] + 0.8 * mToPx(0.5))
-
- # Leftmost Line
- cr.move_to(X_BASE + mToPx(2.89973), Y_BASE + mToPx(3.86305))
- cr.line_to(X_BASE + mToPx(3.15642), Y_BASE + mToPx(3.39548))
-
- # Top Line
- cr.move_to(X_BASE + mToPx(3.15642), Y_BASE + mToPx(3.39548))
- cr.line_to(X_BASE + mToPx(3.63473), Y_BASE + mToPx(3.39238))
-
- #Rightmost Line
- cr.move_to(X_BASE + mToPx(3.63473), Y_BASE + mToPx(3.39238))
- cr.line_to(X_BASE + mToPx(3.89984), Y_BASE + mToPx(3.86305))
-
- #Back Line
- cr.move_to(X_BASE + mToPx(2.89973), Y_BASE + mToPx(3.86305))
- cr.line_to(X_BASE + mToPx(3.89984), Y_BASE + mToPx(3.86305))
-
- # Bottom Rocket
- # Leftmost Line
- cr.move_to(X_BASE + mToPx(2.89973), Y_BASE - mToPx(3.86305))
- cr.line_to(X_BASE + mToPx(3.15642), Y_BASE - mToPx(3.39548))
-
- # Top Line
- cr.move_to(X_BASE + mToPx(3.15642), Y_BASE - mToPx(3.39548))
- cr.line_to(X_BASE + mToPx(3.63473), Y_BASE - mToPx(3.39238))
-
- #Rightmost Line
- cr.move_to(X_BASE + mToPx(3.63473), Y_BASE - mToPx(3.39238))
- cr.line_to(X_BASE + mToPx(3.89984), Y_BASE - mToPx(3.86305))
-
- #Back Line
- cr.move_to(X_BASE + mToPx(2.89973), Y_BASE - mToPx(3.86305))
- cr.line_to(X_BASE + mToPx(3.89984), Y_BASE - mToPx(3.86305))
-
- cr.stroke()
-
-
-def draw_cargo_ship(cr):
- # BASE Constants
- X_BASE = 0 + mToPx(5.59435)
- Y_BASE = 0 + 0 #mToPx(4.129151)
- R = 0.381 - 0.1
-
- FRONT_PEG_DELTA_Y = mToPx(0.276352)
- cr.move_to(X_BASE, Y_BASE + FRONT_PEG_DELTA_Y)
- cr.line_to(X_BASE - mToPx(0.8), Y_BASE + FRONT_PEG_DELTA_Y)
-
- cr.move_to(X_BASE, Y_BASE + FRONT_PEG_DELTA_Y)
- cr.arc(X_BASE - mToPx(R), Y_BASE + FRONT_PEG_DELTA_Y, 5, 0, np.pi * 2.0)
-
- cr.move_to(X_BASE, Y_BASE - FRONT_PEG_DELTA_Y)
- cr.line_to(X_BASE - mToPx(0.8), Y_BASE - FRONT_PEG_DELTA_Y)
-
- cr.move_to(X_BASE, Y_BASE - FRONT_PEG_DELTA_Y)
- cr.arc(X_BASE - mToPx(R), Y_BASE - FRONT_PEG_DELTA_Y, 5, 0, np.pi * 2.0)
-
- SIDE_PEG_Y = mToPx(1.41605 / 2.0)
- SIDE_PEG_X = X_BASE + mToPx(1.148842)
- SIDE_PEG_DX = mToPx(0.55245)
-
- cr.move_to(SIDE_PEG_X, SIDE_PEG_Y)
- cr.line_to(SIDE_PEG_X, SIDE_PEG_Y + mToPx(0.8))
- cr.move_to(SIDE_PEG_X, SIDE_PEG_Y + mToPx(R))
- cr.arc(SIDE_PEG_X, SIDE_PEG_Y + mToPx(R), 5, 0, np.pi * 2.0)
-
- cr.move_to(SIDE_PEG_X + SIDE_PEG_DX, SIDE_PEG_Y)
- cr.line_to(SIDE_PEG_X + SIDE_PEG_DX, SIDE_PEG_Y + mToPx(0.8))
- cr.move_to(SIDE_PEG_X + SIDE_PEG_DX, SIDE_PEG_Y + mToPx(R))
- cr.arc(SIDE_PEG_X + SIDE_PEG_DX, SIDE_PEG_Y + mToPx(R), 5, 0, np.pi * 2.0)
-
- cr.move_to(SIDE_PEG_X + 2.0 * SIDE_PEG_DX, SIDE_PEG_Y)
- cr.line_to(SIDE_PEG_X + 2.0 * SIDE_PEG_DX, SIDE_PEG_Y + mToPx(0.8))
- cr.move_to(SIDE_PEG_X + 2.0 * SIDE_PEG_DX, SIDE_PEG_Y + mToPx(R))
- cr.arc(SIDE_PEG_X + 2.0 * SIDE_PEG_DX, SIDE_PEG_Y + mToPx(R), 5, 0,
- np.pi * 2.0)
-
- cr.move_to(SIDE_PEG_X, -1.0 * SIDE_PEG_Y)
- cr.line_to(SIDE_PEG_X, -1.0 * SIDE_PEG_Y - mToPx(0.8))
- cr.move_to(SIDE_PEG_X, -1.0 * SIDE_PEG_Y - mToPx(R))
- cr.arc(SIDE_PEG_X, -1.0 * SIDE_PEG_Y - mToPx(R), 5, 0, np.pi * 2.0)
-
- cr.move_to(SIDE_PEG_X + SIDE_PEG_DX, -1.0 * SIDE_PEG_Y)
- cr.line_to(SIDE_PEG_X + SIDE_PEG_DX, -1.0 * SIDE_PEG_Y - mToPx(0.8))
- cr.move_to(SIDE_PEG_X + SIDE_PEG_DX, -1.0 * SIDE_PEG_Y - mToPx(R))
- cr.arc(SIDE_PEG_X + SIDE_PEG_DX, -1.0 * SIDE_PEG_Y - mToPx(R), 5, 0,
- np.pi * 2.0)
-
- cr.move_to(SIDE_PEG_X + 2.0 * SIDE_PEG_DX, -1.0 * SIDE_PEG_Y)
- cr.line_to(SIDE_PEG_X + 2.0 * SIDE_PEG_DX, -1.0 * SIDE_PEG_Y - mToPx(0.8))
- cr.move_to(SIDE_PEG_X + 2.0 * SIDE_PEG_DX, -1.0 * SIDE_PEG_Y - mToPx(R))
- cr.arc(SIDE_PEG_X + 2.0 * SIDE_PEG_DX, -1.0 * SIDE_PEG_Y - mToPx(R), 5, 0,
- np.pi * 2.0)
-
- cr.rectangle(X_BASE, Y_BASE - mToPx(1.41605 / 2.0), mToPx(2.43205),
- mToPx(1.41605))
- cr.stroke()
-
-
def draw_points(cr, p, size):
for i in range(0, len(p)):
draw_px_cross(cr, p[i][0], p[i][1], size, Color(
diff --git a/frc971/control_loops/python/path_edit.py b/frc971/control_loops/python/path_edit.py
index d402593..3386bc6 100755
--- a/frc971/control_loops/python/path_edit.py
+++ b/frc971/control_loops/python/path_edit.py
@@ -52,8 +52,6 @@
self.held_x = 0
self.spline_edit = -1
- self.curves = []
-
try:
self.field_png = cairo.ImageSurface.create_from_png(
"frc971/control_loops/python/field_images/" + FIELD.field_id +
@@ -185,30 +183,22 @@
if self.mode == Mode.kPlacing or self.mode == Mode.kViewing:
set_color(cr, palette["BLACK"])
- plotPoints = self.points.getPoints()
- if plotPoints:
- for i, point in enumerate(plotPoints):
- draw_px_x(cr, mToPx(point[0]), mToPx(point[1]), 10)
- cr.move_to(mToPx(point[0]), mToPx(point[1]) - 15)
- display_text(cr, str(i), 0.5, 0.5, 2, 2)
+ for i, point in enumerate(self.points.getPoints()):
+ draw_px_x(cr, mToPx(point[0]), mToPx(point[1]), 10)
set_color(cr, palette["WHITE"])
-
elif self.mode == Mode.kEditing:
set_color(cr, palette["BLACK"])
- cr.move_to(-SCREEN_SIZE, 170)
- display_text(cr, "EDITING", 1, 1, 1, 1)
if self.points.getSplines():
self.draw_splines(cr)
for i, points in enumerate(self.points.getSplines()):
- p0 = np.array([mToPx(points[0][0]), mToPx(points[0][1])])
- p1 = np.array([mToPx(points[1][0]), mToPx(points[1][1])])
- p2 = np.array([mToPx(points[2][0]), mToPx(points[2][1])])
- p3 = np.array([mToPx(points[3][0]), mToPx(points[3][1])])
- p4 = np.array([mToPx(points[4][0]), mToPx(points[4][1])])
- p5 = np.array([mToPx(points[5][0]), mToPx(points[5][1])])
+ points = [
+ np.array([mToPx(x), mToPx(y)])
+ for (x, y) in points
+ ]
+ draw_control_points(cr, points)
- draw_control_points(cr, [p0, p1, p2, p3, p4, p5])
+ p0, p1, p2, p3, p4, p5 = points
first_tangent = p0 + 2.0 * (p1 - p0)
second_tangent = p5 + 2.0 * (p4 - p5)
cr.set_source_rgb(0, 0.5, 0)
@@ -239,7 +229,6 @@
print("spent {:.2f} ms drawing the field widget".format(1000 * (time.perf_counter() - start_time)))
def draw_splines(self, cr):
- holder_spline = []
for i, points in enumerate(self.points.getSplines()):
array = np.zeros(shape=(6, 2), dtype=float)
for j, point in enumerate(points):
@@ -253,15 +242,9 @@
cr.line_to(
mToPx(spline.Point(k)[0]), mToPx(spline.Point(k)[1]))
cr.stroke()
- holding = [
- spline.Point(k - 0.01)[0],
- spline.Point(k - 0.01)[1]
- ]
- holder_spline.append(holding)
if i == 0:
self.draw_robot_at_point(cr, 0.00, 0.01, spline)
self.draw_robot_at_point(cr, 1, 0.01, spline)
- self.curves.append(holder_spline)
def mouse_move(self, event):
old_x = self.mousex
@@ -274,8 +257,9 @@
if self.mode == Mode.kEditing:
self.points.updates_for_mouse_move(self.index_of_edit,
- self.spline_edit, self.mousex,
- self.mousey, difs)
+ self.spline_edit,
+ pxToM(self.mousex),
+ pxToM(self.mousey), difs)
def export_json(self, file_name):
self.path_to_export = os.path.join(
@@ -347,7 +331,8 @@
self.mousey = event.y
if self.mode == Mode.kPlacing:
- if self.points.add_point(self.mousex, self.mousey):
+ if self.points.add_point(
+ pxToM(self.mousex), pxToM(self.mousey)):
self.mode = Mode.kEditing
elif self.mode == Mode.kEditing:
# Now after index_of_edit is not -1, the point is selected, so
diff --git a/frc971/control_loops/python/points.py b/frc971/control_loops/python/points.py
index c22582e..d874306 100644
--- a/frc971/control_loops/python/points.py
+++ b/frc971/control_loops/python/points.py
@@ -70,7 +70,7 @@
def updates_for_mouse_move(self, index_of_edit, spline_edit, x, y, difs):
if index_of_edit > -1:
- self.splines[spline_edit][index_of_edit] = [pxToM(x), pxToM(y)]
+ self.splines[spline_edit][index_of_edit] = [x, y]
if index_of_edit == 5:
self.splines[spline_edit][
@@ -150,7 +150,7 @@
def add_point(self, x, y):
if (len(self.points) < 6):
- self.points.append([pxToM(x), pxToM(y)])
+ self.points.append([x, y])
if (len(self.points) == 6):
self.splines.append(np.array(self.points))
self.points = []
diff --git a/frc971/control_loops/python/spline_graph.py b/frc971/control_loops/python/spline_graph.py
index ad3ae96..1378e8f 100755
--- a/frc971/control_loops/python/spline_graph.py
+++ b/frc971/control_loops/python/spline_graph.py
@@ -12,6 +12,7 @@
import basic_window
import os
+
class GridWindow(Gtk.Window):
def method_connect(self, event, cb):
def handler(obj, *args):
diff --git a/frc971/control_loops/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
index 4250df2..f591847 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -69,8 +69,8 @@
StateFeedbackPlant(
::std::vector<::std::unique_ptr<StateFeedbackPlantCoefficients<
number_of_states, number_of_inputs, number_of_outputs, Scalar>>>
- *coefficients)
- : coefficients_(::std::move(*coefficients)), index_(0) {
+ &&coefficients)
+ : coefficients_(::std::move(coefficients)), index_(0) {
Reset();
}
@@ -222,8 +222,8 @@
explicit StateFeedbackController(
::std::vector<::std::unique_ptr<StateFeedbackControllerCoefficients<
number_of_states, number_of_inputs, number_of_outputs, Scalar>>>
- *controllers)
- : coefficients_(::std::move(*controllers)) {}
+ &&controllers)
+ : coefficients_(::std::move(controllers)) {}
StateFeedbackController(StateFeedbackController &&other)
: index_(other.index_) {
@@ -300,8 +300,8 @@
explicit StateFeedbackObserver(
::std::vector<::std::unique_ptr<StateFeedbackObserverCoefficients<
number_of_states, number_of_inputs, number_of_outputs, Scalar>>>
- *observers)
- : coefficients_(::std::move(*observers)) {}
+ &&observers)
+ : coefficients_(::std::move(observers)) {}
StateFeedbackObserver(StateFeedbackObserver &&other)
: X_hat_(other.X_hat_), index_(other.index_) {
diff --git a/y2020/vision/viewer_replay.cc b/y2020/vision/viewer_replay.cc
index 234c445..93e531d 100644
--- a/y2020/vision/viewer_replay.cc
+++ b/y2020/vision/viewer_replay.cc
@@ -8,8 +8,6 @@
#include "aos/init.h"
#include "y2020/vision/vision_generated.h"
-DEFINE_string(config, "y2020/config.json", "Path to the config file to use.");
-DEFINE_string(logfile, "", "Path to the log file to use.");
DEFINE_string(node, "pi1", "Node name to replay.");
DEFINE_string(image_save_prefix, "/tmp/img",
"Prefix to use for saving images from the logfile.");
@@ -18,13 +16,12 @@
namespace vision {
namespace {
-void ViewerMain() {
- CHECK(!FLAGS_logfile.empty()) << "You forgot to specify a logfile.";
+void ViewerMain(int argc, char *argv[]) {
+ std::vector<std::string> unsorted_logfiles =
+ aos::logger::FindLogs(argc, argv);
- aos::FlatbufferDetachedBuffer<aos::Configuration> config =
- aos::configuration::ReadConfig(FLAGS_config);
-
- aos::logger::LogReader reader(FLAGS_logfile, &config.message());
+ // open logfiles
+ aos::logger::LogReader reader(aos::logger::SortParts(unsorted_logfiles));
reader.Register();
const aos::Node *node = nullptr;
if (aos::configuration::MultiNode(reader.configuration())) {
@@ -61,5 +58,5 @@
// Quick and lightweight grayscale viewer for images
int main(int argc, char **argv) {
aos::InitGoogle(&argc, &argv);
- frc971::vision::ViewerMain();
+ frc971::vision::ViewerMain(argc, argv);
}