Controller Overview
The controller hinges on 3 things:
A novel state estimation pipeline.
Our state of the art implementation of the MPPI algorithm.
An efficient dynamics model that is used for both state estimation and MPPI.
This is implemented in two objects that the node owns - the StateEstimator and the MppiController.
They provide the following functionality:
StateEstimator: given twist and spline, estimates inertial state and a inertial to curvilinear lookup table.
MPPIController: given inertial state and the lookup table, calculates the optimal control action to take.
The inertial state and curvilinear lookup table are shared through GPU global memory.
This diagram shows this relationship:

Implementation
As for the node itself, these are the implementation details:
Initialization: Construction creates subscribers and publishers, then launches MPPI in a new thread.
Callbacks
Spline: Updates the state estimator with the most recent set of spline points. Notifies MPPI thread that the state is dirty.
Twist: Updates the state estimator with the most recent twist. Notifies MPPI thread that the state is dirty.
These callbacks are mutually exclusive.
MPPI Thread: Loops continuously. Waits to be notified that state is dirty (i.e. new incoming message). When notified, does state estimation, then runs MPPI. Publishes the control action and controller info.
Class reference can be found in Class ControllerNode.
Code for the node can be found in controls/src/nodes/controller.cpp
.
Runtime Variables
We found that the compilation times to be a nontrivial slow down to our very limited track tests. A single compilation after a controller parameter change takes approximately 1 minute. To address this, we implemented runtime parameters, where some of the parameters which don’t live on the GPU/interface with CUDA were changed into runtime parameters that were defined in configuration files.
This reduced build times and test times, as we could change the relevant parameters in the configuration file and then run the controller without having to rebuild.