Merge "Improve performance of the y2020 localizer"
diff --git a/README.md b/README.md
index d54d963..f81d423 100644
--- a/README.md
+++ b/README.md
@@ -26,7 +26,8 @@
1. Install the required packages:
```console
apt-get update
-apt-get install bazel python
+# TODO(james): Ideally, we shouldn't need to be installing libtinfo5...
+apt-get install bazel python libtinfo5
```
2. Allow Bazel's sandboxing to work:
Follow the direction in `doc/frc971.conf`.
diff --git a/aos/events/shm_event_loop.cc b/aos/events/shm_event_loop.cc
index b53be2f..ebfcc07 100644
--- a/aos/events/shm_event_loop.cc
+++ b/aos/events/shm_event_loop.cc
@@ -47,6 +47,8 @@
namespace aos {
+using namespace shm_event_loop_internal;
+
void SetShmBase(const std::string_view base) {
FLAGS_shm_base = std::string(base) + "/dev/shm/aos";
}
@@ -82,7 +84,7 @@
// that fails, the file has already been created and we can open it
// normally.. Once the file has been created it wil never be deleted.
int fd = open(path.c_str(), O_RDWR | O_CREAT | O_EXCL,
- O_CLOEXEC | FLAGS_permissions);
+ O_CLOEXEC | FLAGS_permissions);
if (fd == -1 && errno == EEXIST) {
VLOG(1) << path << " already created.";
// File already exists.
@@ -116,9 +118,7 @@
ipc_lib::InitializeLocklessQueueMemory(memory(), config_);
}
- ~MMapedQueue() {
- PCHECK(munmap(data_, size_) == 0);
- }
+ ~MMapedQueue() { PCHECK(munmap(data_, size_) == 0); }
ipc_lib::LocklessQueueMemory *memory() const {
return reinterpret_cast<ipc_lib::LocklessQueueMemory *>(data_);
@@ -160,7 +160,7 @@
}
}
-namespace internal {
+namespace shm_event_loop_internal {
class SimpleShmFetcher {
public:
@@ -433,18 +433,18 @@
};
// Class to manage the state for a Watcher.
-class WatcherState : public aos::WatcherState {
+class ShmWatcherState : public WatcherState {
public:
- WatcherState(
+ ShmWatcherState(
ShmEventLoop *event_loop, const Channel *channel,
std::function<void(const Context &context, const void *message)> fn,
bool copy_data)
- : aos::WatcherState(event_loop, channel, std::move(fn)),
+ : WatcherState(event_loop, channel, std::move(fn)),
event_loop_(event_loop),
event_(this),
simple_shm_fetcher_(event_loop, channel, copy_data) {}
- ~WatcherState() override { event_loop_->RemoveEvent(&event_); }
+ ~ShmWatcherState() override { event_loop_->RemoveEvent(&event_); }
void Startup(EventLoop *event_loop) override {
simple_shm_fetcher_.PointAtNextQueueIndex();
@@ -489,14 +489,14 @@
bool has_new_data_ = false;
ShmEventLoop *event_loop_;
- EventHandler<WatcherState> event_;
+ EventHandler<ShmWatcherState> event_;
SimpleShmFetcher simple_shm_fetcher_;
};
// Adapter class to adapt a timerfd to a TimerHandler.
-class TimerHandlerState final : public TimerHandler {
+class ShmTimerHandler final : public TimerHandler {
public:
- TimerHandlerState(ShmEventLoop *shm_event_loop, ::std::function<void()> fn)
+ ShmTimerHandler(ShmEventLoop *shm_event_loop, ::std::function<void()> fn)
: TimerHandler(shm_event_loop, std::move(fn)),
shm_event_loop_(shm_event_loop),
event_(this) {
@@ -510,7 +510,7 @@
});
}
- ~TimerHandlerState() {
+ ~ShmTimerHandler() {
Disable();
shm_event_loop_->epoll_.DeleteFd(timerfd_.fd());
}
@@ -562,21 +562,22 @@
private:
ShmEventLoop *shm_event_loop_;
- EventHandler<TimerHandlerState> event_;
+ EventHandler<ShmTimerHandler> event_;
- TimerFd timerfd_;
+ internal::TimerFd timerfd_;
monotonic_clock::time_point base_;
monotonic_clock::duration repeat_offset_;
};
// Adapter class to the timerfd and PhasedLoop.
-class PhasedLoopHandler final : public ::aos::PhasedLoopHandler {
+class ShmPhasedLoopHandler final : public PhasedLoopHandler {
public:
- PhasedLoopHandler(ShmEventLoop *shm_event_loop, ::std::function<void(int)> fn,
- const monotonic_clock::duration interval,
- const monotonic_clock::duration offset)
- : aos::PhasedLoopHandler(shm_event_loop, std::move(fn), interval, offset),
+ ShmPhasedLoopHandler(ShmEventLoop *shm_event_loop,
+ ::std::function<void(int)> fn,
+ const monotonic_clock::duration interval,
+ const monotonic_clock::duration offset)
+ : PhasedLoopHandler(shm_event_loop, std::move(fn), interval, offset),
shm_event_loop_(shm_event_loop),
event_(this) {
shm_event_loop_->epoll_.OnReadable(
@@ -598,7 +599,7 @@
});
}
- ~PhasedLoopHandler() override {
+ ~ShmPhasedLoopHandler() override {
shm_event_loop_->epoll_.DeleteFd(timerfd_.fd());
shm_event_loop_->RemoveEvent(&event_);
}
@@ -616,11 +617,12 @@
}
ShmEventLoop *shm_event_loop_;
- EventHandler<PhasedLoopHandler> event_;
+ EventHandler<ShmPhasedLoopHandler> event_;
- TimerFd timerfd_;
+ internal::TimerFd timerfd_;
};
-} // namespace internal
+
+} // namespace shm_event_loop_internal
::std::unique_ptr<RawFetcher> ShmEventLoop::MakeRawFetcher(
const Channel *channel) {
@@ -631,14 +633,14 @@
"configuration.";
}
- return ::std::unique_ptr<RawFetcher>(new internal::ShmFetcher(this, channel));
+ return ::std::unique_ptr<RawFetcher>(new ShmFetcher(this, channel));
}
::std::unique_ptr<RawSender> ShmEventLoop::MakeRawSender(
const Channel *channel) {
TakeSender(channel);
- return ::std::unique_ptr<RawSender>(new internal::ShmSender(this, channel));
+ return ::std::unique_ptr<RawSender>(new ShmSender(this, channel));
}
void ShmEventLoop::MakeRawWatcher(
@@ -647,7 +649,7 @@
TakeWatcher(channel);
NewWatcher(::std::unique_ptr<WatcherState>(
- new internal::WatcherState(this, channel, std::move(watcher), true)));
+ new ShmWatcherState(this, channel, std::move(watcher), true)));
}
void ShmEventLoop::MakeRawNoArgWatcher(
@@ -655,7 +657,7 @@
std::function<void(const Context &context)> watcher) {
TakeWatcher(channel);
- NewWatcher(::std::unique_ptr<WatcherState>(new internal::WatcherState(
+ NewWatcher(::std::unique_ptr<WatcherState>(new ShmWatcherState(
this, channel,
[watcher](const Context &context, const void *) { watcher(context); },
false)));
@@ -663,16 +665,15 @@
TimerHandler *ShmEventLoop::AddTimer(::std::function<void()> callback) {
return NewTimer(::std::unique_ptr<TimerHandler>(
- new internal::TimerHandlerState(this, ::std::move(callback))));
+ new ShmTimerHandler(this, ::std::move(callback))));
}
PhasedLoopHandler *ShmEventLoop::AddPhasedLoop(
::std::function<void(int)> callback,
const monotonic_clock::duration interval,
const monotonic_clock::duration offset) {
- return NewPhasedLoop(
- ::std::unique_ptr<PhasedLoopHandler>(new internal::PhasedLoopHandler(
- this, ::std::move(callback), interval, offset)));
+ return NewPhasedLoop(::std::unique_ptr<PhasedLoopHandler>(
+ new ShmPhasedLoopHandler(this, ::std::move(callback), interval, offset)));
}
void ShmEventLoop::OnRun(::std::function<void()> on_run) {
@@ -682,8 +683,8 @@
void ShmEventLoop::HandleEvent() {
// Update all the times for handlers.
for (::std::unique_ptr<WatcherState> &base_watcher : watchers_) {
- internal::WatcherState *watcher =
- reinterpret_cast<internal::WatcherState *>(base_watcher.get());
+ ShmWatcherState *watcher =
+ reinterpret_cast<ShmWatcherState *>(base_watcher.get());
watcher->CheckForNewData();
}
@@ -860,8 +861,8 @@
}
for (::std::unique_ptr<WatcherState> &base_watcher : watchers_) {
- internal::WatcherState *watcher =
- reinterpret_cast<internal::WatcherState *>(base_watcher.get());
+ ShmWatcherState *watcher =
+ reinterpret_cast<ShmWatcherState *>(base_watcher.get());
watcher->UnregisterWakeup();
}
@@ -909,14 +910,14 @@
}
absl::Span<char> ShmEventLoop::GetWatcherSharedMemory(const Channel *channel) {
- internal::WatcherState *const watcher_state =
- static_cast<internal::WatcherState *>(GetWatcherState(channel));
+ ShmWatcherState *const watcher_state =
+ static_cast<ShmWatcherState *>(GetWatcherState(channel));
return watcher_state->GetSharedMemory();
}
absl::Span<char> ShmEventLoop::GetShmSenderSharedMemory(
const aos::RawSender *sender) const {
- return static_cast<const internal::ShmSender *>(sender)->GetSharedMemory();
+ return static_cast<const ShmSender *>(sender)->GetSharedMemory();
}
pid_t ShmEventLoop::GetTid() { return syscall(SYS_gettid); }
diff --git a/aos/events/shm_event_loop.h b/aos/events/shm_event_loop.h
index 15759f4..f832242 100644
--- a/aos/events/shm_event_loop.h
+++ b/aos/events/shm_event_loop.h
@@ -10,15 +10,15 @@
#include "aos/events/event_loop_generated.h"
namespace aos {
-namespace internal {
+namespace shm_event_loop_internal {
-class WatcherState;
-class TimerHandlerState;
-class PhasedLoopHandler;
+class ShmWatcherState;
+class ShmTimerHandler;
+class ShmPhasedLoopHandler;
class ShmSender;
class ShmFetcher;
-} // namespace internal
+} // namespace shm_event_loop_internal
// Specialization of EventLoop that is built from queues running out of shared
// memory.
@@ -57,11 +57,10 @@
std::function<void(const Context &context)> watcher) override;
TimerHandler *AddTimer(std::function<void()> callback) override;
- aos::PhasedLoopHandler *AddPhasedLoop(
- std::function<void(int)> callback,
- const monotonic_clock::duration interval,
- const monotonic_clock::duration offset =
- std::chrono::seconds(0)) override;
+ PhasedLoopHandler *AddPhasedLoop(std::function<void(int)> callback,
+ const monotonic_clock::duration interval,
+ const monotonic_clock::duration offset =
+ std::chrono::seconds(0)) override;
void OnRun(std::function<void()> on_run) override;
@@ -89,11 +88,11 @@
}
private:
- friend class internal::WatcherState;
- friend class internal::TimerHandlerState;
- friend class internal::PhasedLoopHandler;
- friend class internal::ShmSender;
- friend class internal::ShmFetcher;
+ friend class shm_event_loop_internal::ShmWatcherState;
+ friend class shm_event_loop_internal::ShmTimerHandler;
+ friend class shm_event_loop_internal::ShmPhasedLoopHandler;
+ friend class shm_event_loop_internal::ShmSender;
+ friend class shm_event_loop_internal::ShmFetcher;
static cpu_set_t DefaultAffinity() {
cpu_set_t result;
diff --git a/documentation/tutorials/BUILD b/documentation/tutorials/BUILD
index 13516c1..5816444 100644
--- a/documentation/tutorials/BUILD
+++ b/documentation/tutorials/BUILD
@@ -6,10 +6,8 @@
"create-a-simple-program-for-running-a-motor",
"download-code-to-the-robot",
"make-a-drivebase-move",
- "send-and-receive-messages-on-queues",
"submitting-code-for-a-review",
"tune-an-autonomous",
- "generated-queue-code",
"using-bazel",
]
diff --git a/documentation/tutorials/generated-queue-code.md b/documentation/tutorials/generated-queue-code.md
deleted file mode 100644
index 8f00a31..0000000
--- a/documentation/tutorials/generated-queue-code.md
+++ /dev/null
@@ -1,246 +0,0 @@
-# Generated queue code
-
-This tutorial covers what the code that is generated from a .q file looks like.
-
-### The basics
-Each `.q` file generates a `.cc` and `.h` pair that can be #include'd in your c++
-code.
-
-We're going to use `//frc971/queues:gyro.q` as our example.
-
-## Package declaration
-`.q` files usually start with a `package` declaration. The `package` declaration
-directly converts to a set of namespace declarations. So
-
-```cpp
-package frc971.sensors;
-```
-
-generates the namespace definitions
-
-```cpp
-namespace frc971 {
-namespace sensors {
-
-// All of the code for the queue
-
-} // namespace sensors
-} // namespace frc971
-```
-
-## Message declarations
-Each message declared in the queue generates a class that can be instantiated in
-your code easily.
-
-For a simple example, lets use the `Uid` message in `//frc971/queues:gyro.q`:
-
-```cpp
-message Uid {
- uint32_t uid;
-};
-```
-
-Let's take a look at the actual class definition that is created. If you ever
-want to inspect the code that is generated from a .q file, you can take a look
-in `bazel-genfiles` after you build your code. So after running `bazel build
-//frc971/queues:gyro` there will be a `bazel-genfiles/frc971/queues/gyro.q.h`
-and `bazel-genfiles/frc971/queues/gyro.q.cc`.
-
-Here's the definition of the `Uid` class from
-`bazel-genfiles/frc971/queues/gyro.q.h` (comments mine):
-
-```cpp
-struct Uid : public ::aos::Message {
- // Used to identify queues uniquely even if they have the same name.
- enum { kQueueLength = 100, kHash = 0x0837c541 };
-
- // The actual data that we requested be part of the message.
- uint32_t uid;
-
- // Writes the message to a byte buffer.
- size_t Serialize(char *buffer) const;
- // Reads the message from a byte buffer.
- size_t Deserialize(const char *buffer);
- // Zeroes all of the fields in the message.
- void Zero();
- // Returns the size of message. Not used
- static size_t Size() { return 4 + ::aos::Message::Size(); }
- // Prints the contents of the message to a string that is human readable.
- size_t Print(char *buffer, size_t length) const;
-
- // Gets information about this message's type.
- static const ::aos::MessageType *GetType();
- static const ::aos::MessageType *DoGetType();
-
- // Default constructor, zeroes the struct.
- Uid();
- // Value constructor, sets the data at construction.
- Uid(uint32_t uid_in);
- bool EqualsNoTime(const Uid &other) const;
-};
-```
-
-## Queue declarations
-Every `queue` declaration in the `.q` file creates an `aos::Queue` variable that
-uses the message type given. So the declaration
-
-```cpp
-queue Uid gyro_part_id;
-```
-
-will generate a variable in `gyro.q.h`
-
-```cpp
-static UNUSED_VARIABLE::aos::Queue<Uid> &gyro_part_id;
-```
-
-which can then be used in any file that includes the header file.
-
-
-## QueueGroup declarations
-
-`queue_group` declares a set of queues and some associated helpers for them.
-There are two kinds of `queue_groups`, declarations and aliases. Here's an
-example of a `queue_group` definition:
-
-```cpp
-queue_group SuperstructureQueue {
- implements aos.control_loops.ControlLoop;
-
- message Goal {
- IntakeGoal intake;
-
- // Used to identify a position in the planned set of positions on the arm.
- uint32_t arm_goal_position;
- // If true, start the grab box sequence.
- bool grab_box;
-
- bool open_claw;
- bool close_claw;
-
- bool deploy_fork;
-
- bool hook_release;
-
- double voltage_winch;
-
- double open_threshold;
-
- bool disable_box_correct;
-
- bool trajectory_override;
- };
-
- message Status {
- // Are all the subsystems zeroed?
- bool zeroed;
-
- // If true, any of the subsystems have aborted.
- bool estopped;
-
- // Status of both intake sides.
- IntakeSideStatus left_intake;
- IntakeSideStatus right_intake;
-
- ArmStatus arm;
-
- double filtered_box_velocity;
- uint32_t rotation_state;
- };
-
- message Position {
- // Values of the series elastic encoders on the left side of the robot from
- // the rear perspective in radians.
- IntakeElasticSensors left_intake;
-
- // Values of the series elastic encoders on the right side of the robot from
- // the rear perspective in radians.
- IntakeElasticSensors right_intake;
-
- ArmPosition arm;
-
- // Value of the beam breaker sensor. This value is true if the beam is
- // broken, false if the beam isn't broken.
- bool claw_beambreak_triggered;
- // Value of the beambreak sensor detecting when the box has hit the frame
- // cutout.
- bool box_back_beambreak_triggered;
-
- // Distance to the box in meters.
- double box_distance;
- };
-
- message Output {
- // Voltage sent to the parts on the left side of the intake.
- IntakeVoltage left_intake;
-
- // Voltage sent to the parts on the right side of the intake.
- IntakeVoltage right_intake;
-
- // Voltage sent to the motors on the proximal joint of the arm.
- double voltage_proximal;
-
- // Voltage sent to the motors on the distal joint of the arm.
- double voltage_distal;
-
- // Voltage sent to the hanger. Positive pulls the robot up.
- double voltage_winch;
-
- // Clamped (when true) or unclamped (when false) status sent to the
- // pneumatic claw on the arm.
- bool claw_grabbed;
-
- // If true, release the arm brakes.
- bool release_arm_brake;
- // If true, release the hook
- bool hook_release;
- // If true, release the forks
- bool forks_release;
- };
-
- queue Goal goal;
- queue Output output;
- queue Status status;
- queue Position position;
-};
-```
-
-and aliases look like:
-
-```cpp
-queue_group SuperstructureQueue superstructure_queue;
-```
-
-The declaration type creates the definition of the queue group and what messages
-and queues it contains. The alias type creates a variable with the given name.
-
-This queue group results in the following code in
-`bazel-genfiles/y2018/control_loops/superstructure/superstructure.q.h`
-
-```cpp
-
-struct SuperstructureQueue_Goal : public ::aos::Message { /* ... */ }
-struct SuperstructureQueue_Output : public ::aos::Message { /* ... */ }
-struct SuperstructureQueue_Status : public ::aos::Message { /* ... */ }
-struct SuperstructureQueue_Position : public ::aos::Message { /* ... */ }
-class SuperstructureQueue : public ::aos::QueueGroup {
- public:
- typedef SuperstructureQueue_Goal Goal;
- ::aos::Queue<SuperstructureQueue_Goal> goal;
- typedef SuperstructureQueue_Output Output;
- ::aos::Queue<SuperstructureQueue_Output> output;
- typedef SuperstructureQueue_Status Status;
- ::aos::Queue<SuperstructureQueue_Status> status;
- typedef SuperstructureQueue_Position Position;
- ::aos::Queue<SuperstructureQueue_Position> position;
- SuperstructureQueue(const char *name, uint32_t hash, const char *goal_name,
- const char *output_name, const char *status_name,
- const char *position_name);
-};
-```
-
-and a definition of a variable:
-
-```cpp
-static UNUSED_VARIABLE SuperstructureQueue &superstructure_queue;
-```
diff --git a/documentation/tutorials/send-and-receive-messages-on-queues.md b/documentation/tutorials/send-and-receive-messages-on-queues.md
deleted file mode 100644
index bcdaac1..0000000
--- a/documentation/tutorials/send-and-receive-messages-on-queues.md
+++ /dev/null
@@ -1,74 +0,0 @@
-# How to send and receive messages on a Queue
-
-Let's say we have a `//y2018:basic_queue.q` file that has the following
-contents:
-
-```cpp
-package y2018;
-
-message BasicMessage {
- float value;
-};
-
-queue BasicMessage basic_queue;
-```
-
-and the corresponding entry in `//y2018/BUILD`
-
-```python
-queue_library(
- name = "basic_queue",
- srcs = [
- "basic_queue.q",
- ],
- visibility = ["//visibility:public"],
-)
-```
-
-The following examples assume that you've declared a dependency on
-`//y2018:basic_queue` in the relevant `BUILD` file.
-
-## Sending a message
-First, include the queue definition that is created from the `.q` file. This
-will make the `basic_queue` variable available.
-
-```cpp
-#include "y2018/basic_queue.q.h"
-```
-
-```cpp
-
-// Allocate a new message pointer to use. basic_queue is from the include
-// statement.
-auto new_basic_message = basic_queue.MakeMessage();
-new_goal.value = 0.5;
-
-// Send the message on the queue
-if (!new_basic_message.Send()) {
- LOG(ERROR, "Failed to send.\n");
-}
-```
-
-## Receiving a message
-
-Once again, we must include the queue header file to get access to the queue.
-
-
-```cpp
-#include "y2018/basic_queue.q.h"
-```
-
-Then we can receive a message.
-
-
-```cpp
-basic_queue.FetchLatest();
-if (basic_queue.get()) {
- // We have a valid message ready to be used
- LOG(INFO, "Got a BasicMessage from basic_queue with value %f\n",
- basic_queue->value);
-} else {
- // No new message was available
- LOG(ERROR, "No message was received\n");
-}
-```