diff --git a/include/scrimmage/Hash.h b/include/scrimmage/Hash.h
index 2aefb4db90..fcea37a06d 100644
--- a/include/scrimmage/Hash.h
+++ b/include/scrimmage/Hash.h
@@ -37,15 +37,14 @@
 // custom specialization of std::hash can be injected in namespace std
 // see http://en.cppreference.com/w/cpp/utility/hash
 namespace std {
-template<> struct hash<scrimmage::ID> {
+template <>
+struct hash<scrimmage::ID> {
     size_t operator()(scrimmage::ID const& id) const {
         // assume up to 16 teams and 64 subswarms
         // for a 32-bit int this leaves 16 bits (up to 65000 ids)
-        return id.team_id() &
-            (id.sub_swarm_id() << 4) &
-            (id.id() << 12);
+        return id.team_id() & (id.sub_swarm_id() << 4) & (id.id() << 12);
     }
 };
-} // namespace std
+}  // namespace std
 
-#endif // INCLUDE_SCRIMMAGE_HASH_H_
+#endif  // INCLUDE_SCRIMMAGE_HASH_H_
diff --git a/include/scrimmage/autonomy/Autonomy.h b/include/scrimmage/autonomy/Autonomy.h
index 8760bfe45f..76d3dfd826 100644
--- a/include/scrimmage/autonomy/Autonomy.h
+++ b/include/scrimmage/autonomy/Autonomy.h
@@ -33,8 +33,8 @@
 #ifndef INCLUDE_SCRIMMAGE_AUTONOMY_AUTONOMY_H_
 #define INCLUDE_SCRIMMAGE_AUTONOMY_AUTONOMY_H_
 
-#include <scrimmage/fwd_decl.h>
 #include <scrimmage/entity/EntityPlugin.h>
+#include <scrimmage/fwd_decl.h>
 
 #include <list>
 #include <map>
@@ -51,28 +51,30 @@ class Autonomy : public EntityPlugin {
     virtual bool step_autonomy(double t, double dt);
     virtual bool posthumous(double t);
     virtual void init();
-    bool ready() override { return true; }
-    virtual void init(std::map<std::string, std::string> &params);
+    bool ready() override {
+        return true;
+    }
+    virtual void init(std::map<std::string, std::string>& params);
     void close(double t) override;
     bool need_reset();
 
     // getters/setters
-    StatePtr &desired_state();
+    StatePtr& desired_state();
     void set_desired_state(StatePtr desired_state);
 
-    ContactMapPtr &get_contacts();
-    ContactMap &get_contacts_raw();
-    virtual void set_contacts(ContactMapPtr &contacts);
-    virtual void set_contacts_from_plugin(AutonomyPtr &ptr);
-    virtual void set_projection(std::shared_ptr<GeographicLib::LocalCartesian> &proj);
+    ContactMapPtr& get_contacts();
+    ContactMap& get_contacts_raw();
+    virtual void set_contacts(ContactMapPtr& contacts);
+    virtual void set_contacts_from_plugin(AutonomyPtr& ptr);
+    virtual void set_projection(std::shared_ptr<GeographicLib::LocalCartesian>& proj);
 
-    scrimmage::RTreePtr &rtree();
-    void set_rtree(scrimmage::RTreePtr &rtree);
+    scrimmage::RTreePtr& rtree();
+    void set_rtree(scrimmage::RTreePtr& rtree);
 
-    StatePtr &state();
-    virtual void set_state(StatePtr &state);
+    StatePtr& state();
+    virtual void set_state(StatePtr& state);
 
-    std::string &logging_msg();
+    std::string& logging_msg();
 
     bool get_is_controlling();
     void set_is_controlling(bool is_controlling);
@@ -90,5 +92,5 @@ class Autonomy : public EntityPlugin {
 
     bool is_controlling_;
 };
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_AUTONOMY_AUTONOMY_H_
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_AUTONOMY_AUTONOMY_H_
diff --git a/include/scrimmage/common/Algorithm.h b/include/scrimmage/common/Algorithm.h
index 15ecd5b645..00acc1cb82 100644
--- a/include/scrimmage/common/Algorithm.h
+++ b/include/scrimmage/common/Algorithm.h
@@ -33,8 +33,8 @@
 #ifndef INCLUDE_SCRIMMAGE_COMMON_ALGORITHM_H_
 #define INCLUDE_SCRIMMAGE_COMMON_ALGORITHM_H_
 
-#include <iterator>
 #include <algorithm>
+#include <iterator>
 #include <unordered_set>
 
 namespace scrimmage {
@@ -43,7 +43,7 @@ namespace scrimmage {
  * This implementation assumes the first argument has an erase method.
  */
 template <class ContainerType, class Predicate>
-void remove_if(ContainerType &container, Predicate pred) {
+void remove_if(ContainerType& container, Predicate pred) {
     auto it = container.begin();
     while (it != container.end()) {
         it = pred(*it) ? container.erase(it) : std::next(it);
@@ -55,29 +55,29 @@ void remove_if(ContainerType &container, Predicate pred) {
  * std::set_difference does not work on unordered_sets
  */
 template <class T = std::unordered_set<int>>
-std::unordered_set<T>
-set_difference(const std::unordered_set<T> &container1, const std::unordered_set<T> &container2) {
+std::unordered_set<T> set_difference(const std::unordered_set<T>& container1,
+                                     const std::unordered_set<T>& container2) {
     std::unordered_set<T> out;
     std::copy_if(container1.begin(), container1.end(), std::inserter(out, out.end()),
-        [&](const T &val) {return container2.count(val) == 0;});
+                 [&](const T& val) { return container2.count(val) == 0; });
     return out;
 }
 
 template <class T = std::unordered_set<int>>
-std::unordered_set<T>
-set_union(const std::unordered_set<T> &container1, const std::unordered_set<T> &container2) {
+std::unordered_set<T> set_union(const std::unordered_set<T>& container1,
+                                const std::unordered_set<T>& container2) {
     std::unordered_set<T> out = container1;
     out.insert(container2.begin(), container2.end());
     return out;
 }
 
 template <class T = std::unordered_set<int>>
-std::unordered_set<T>
-set_intersection(const std::unordered_set<T> &container1, const std::unordered_set<T> &container2) {
+std::unordered_set<T> set_intersection(const std::unordered_set<T>& container1,
+                                       const std::unordered_set<T>& container2) {
     std::unordered_set<T> out;
     std::copy_if(container1.begin(), container1.end(), std::inserter(out, out.end()),
-        [&](const T &val) {return container2.count(val) != 0;});
+                 [&](const T& val) { return container2.count(val) != 0; });
     return out;
 }
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_COMMON_ALGORITHM_H_
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_COMMON_ALGORITHM_H_
diff --git a/include/scrimmage/common/Battery.h b/include/scrimmage/common/Battery.h
index 3dda06d120..c77afc067e 100644
--- a/include/scrimmage/common/Battery.h
+++ b/include/scrimmage/common/Battery.h
@@ -36,13 +36,14 @@
 namespace scrimmage {
 class Battery {
  public:
-    Battery() {}
-    Battery(const double &min, const double &max, const double &current);
-    void add_charge(const double &amount);
-    bool deplete(const double &amount);
+    Battery() {
+    }
+    Battery(const double& min, const double& max, const double& current);
+    void add_charge(const double& amount);
+    bool deplete(const double& amount);
     bool is_full();
     bool has_charge();
-    const double &current_charge();
+    const double& current_charge();
     double charge_percentage();
 
  protected:
@@ -50,5 +51,5 @@ class Battery {
     double max_charge_ = 1;
     double current_charge_ = 1;
 };
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_COMMON_BATTERY_H_
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_COMMON_BATTERY_H_
diff --git a/include/scrimmage/common/CSV.h b/include/scrimmage/common/CSV.h
index d9a44d611c..13a522c9b0 100644
--- a/include/scrimmage/common/CSV.h
+++ b/include/scrimmage/common/CSV.h
@@ -34,11 +34,11 @@
 #define INCLUDE_SCRIMMAGE_COMMON_CSV_H_
 
 #include <fstream>
-#include <map>
 #include <list>
+#include <map>
+#include <memory>
 #include <string>
 #include <utility>
-#include <memory>
 
 namespace scrimmage {
 
@@ -49,45 +49,50 @@ class CSV {
 
     ~CSV();
 
-    void set_column_headers(const Headers &headers, bool write = true);
+    void set_column_headers(const Headers& headers, bool write = true);
 
-    void set_column_headers(const std::string &headers, bool write = true);
+    void set_column_headers(const std::string& headers, bool write = true);
 
-    bool append(const Pairs &pairs, bool write = true, bool keep_in_memory = false);
+    bool append(const Pairs& pairs, bool write = true, bool keep_in_memory = false);
 
-    bool open_output(const std::string &filename,
-                     std::ios_base::openmode mode = (std::ios_base::out |
-                                                     std::ios_base::trunc));
+    bool open_output(const std::string& filename,
+                     std::ios_base::openmode mode = (std::ios_base::out | std::ios_base::trunc));
 
     bool output_is_open();
 
     bool close_output();
 
-    bool to_csv(const std::string &filename);
+    bool to_csv(const std::string& filename);
 
-    bool read_csv(const std::string &filename, const bool& contains_header = true);
+    bool read_csv(const std::string& filename, const bool& contains_header = true);
 
-    bool read_csv_from_string(const std::string &str, const bool& contains_header = true);
+    bool read_csv_from_string(const std::string& str, const bool& contains_header = true);
 
-    void set_no_value_string(const std::string &str);
+    void set_no_value_string(const std::string& str);
 
     std::string to_string() const;
 
     size_t rows();
 
-    double at(int row, const std::string &header);
+    double at(int row, const std::string& header);
 
     friend std::ostream& operator<<(std::ostream& os, const CSV& csv);
 
     bool equals(const CSV& other);
 
     // double parameters
-    void set_double_precision(int precision) { double_precision_ = precision; }
-    void set_double_fixed(bool is_fixed) { double_is_fixed_ = is_fixed; }
-    void set_double_scientific(bool is_scientific) { double_is_scientific_ = is_scientific; }
+    void set_double_precision(int precision) {
+        double_precision_ = precision;
+    }
+    void set_double_fixed(bool is_fixed) {
+        double_is_fixed_ = is_fixed;
+    }
+    void set_double_scientific(bool is_scientific) {
+        double_is_scientific_ = is_scientific;
+    }
 
  protected:
-    std::list<std::string> get_csv_line_elements(const std::string &str);
+    std::list<std::string> get_csv_line_elements(const std::string& str);
 
     void write_headers();
 
@@ -115,6 +120,6 @@ class CSV {
     std::string rows_to_string() const;
     std::string row_to_string(const int& i) const;
 };
-} // namespace scrimmage
+}  // namespace scrimmage
 
-#endif // INCLUDE_SCRIMMAGE_COMMON_CSV_H_
+#endif  // INCLUDE_SCRIMMAGE_COMMON_CSV_H_
diff --git a/include/scrimmage/common/ColorMaps.h b/include/scrimmage/common/ColorMaps.h
index 80801e6653..b023b413c1 100644
--- a/include/scrimmage/common/ColorMaps.h
+++ b/include/scrimmage/common/ColorMaps.h
@@ -51,6 +51,6 @@ Color_t GetColor(double v, double vmin, double vmax);
 
 int GetGray(Color_t c, double vmin, double vmax);
 
-} // namespace scrimmage
+}  // namespace scrimmage
 
-#endif // INCLUDE_SCRIMMAGE_COMMON_COLORMAPS_H_
+#endif  // INCLUDE_SCRIMMAGE_COMMON_COLORMAPS_H_
diff --git a/include/scrimmage/common/DelayedTask.h b/include/scrimmage/common/DelayedTask.h
index 114431fb52..a126d057bc 100644
--- a/include/scrimmage/common/DelayedTask.h
+++ b/include/scrimmage/common/DelayedTask.h
@@ -32,8 +32,8 @@
 #ifndef INCLUDE_SCRIMMAGE_COMMON_DELAYEDTASK_H_
 #define INCLUDE_SCRIMMAGE_COMMON_DELAYEDTASK_H_
 
-#include <utility>
 #include <functional>
+#include <utility>
 
 namespace scrimmage {
 
@@ -64,5 +64,5 @@ class DelayedTask {
     bool repeat_infinitely_;
     int repeats_left_;
 };
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_COMMON_DELAYEDTASK_H_
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_COMMON_DELAYEDTASK_H_
diff --git a/include/scrimmage/common/ExponentialFilter.h b/include/scrimmage/common/ExponentialFilter.h
index 4590746259..f343a96a1b 100644
--- a/include/scrimmage/common/ExponentialFilter.h
+++ b/include/scrimmage/common/ExponentialFilter.h
@@ -37,17 +37,17 @@ namespace scrimmage {
 
 class ExponentialFilter {
  public:
-     ExponentialFilter();
-     explicit ExponentialFilter(double time_constant);
-     void set_time_constant(double time_constant);
-     double add_estimate(double estimate, double t);
-     double get_estimate() const;
+    ExponentialFilter();
+    explicit ExponentialFilter(double time_constant);
+    void set_time_constant(double time_constant);
+    double add_estimate(double estimate, double t);
+    double get_estimate() const;
 
  protected:
-     double estimate_;
-     double time_last_estimate_;
-     double time_constant_;
+    double estimate_;
+    double time_last_estimate_;
+    double time_constant_;
 };
 
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_COMMON_EXPONENTIALFILTER_H_
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_COMMON_EXPONENTIALFILTER_H_
diff --git a/include/scrimmage/common/FSMBehavior.h b/include/scrimmage/common/FSMBehavior.h
index 9a34c4ec78..c174e45cf0 100644
--- a/include/scrimmage/common/FSMBehavior.h
+++ b/include/scrimmage/common/FSMBehavior.h
@@ -36,20 +36,24 @@
 #include <scrimmage/autonomy/Autonomy.h>
 
 #include <map>
-#include <string>
 #include <memory>
+#include <string>
 
 namespace scrimmage {
 namespace common {
 class FSMBehavior : public scrimmage::Autonomy {
  public:
-    virtual void entered() {}
-    virtual void step_inactive() {}
-    virtual void exited() {}
+    virtual void entered() {
+    }
+    virtual void step_inactive() {
+    }
+    virtual void exited() {
+    }
+
  protected:
 };
 using FSMBehaviorPtr = std::shared_ptr<FSMBehavior>;
-} // namespace common
-} // namespace scrimmage
+}  // namespace common
+}  // namespace scrimmage
 
-#endif // INCLUDE_SCRIMMAGE_COMMON_FSMBEHAVIOR_H_
+#endif  // INCLUDE_SCRIMMAGE_COMMON_FSMBEHAVIOR_H_
diff --git a/include/scrimmage/common/FileSearch.h b/include/scrimmage/common/FileSearch.h
index 6707c8411c..2b47a9936a 100644
--- a/include/scrimmage/common/FileSearch.h
+++ b/include/scrimmage/common/FileSearch.h
@@ -33,13 +33,14 @@
 #ifndef INCLUDE_SCRIMMAGE_COMMON_FILESEARCH_H_
 #define INCLUDE_SCRIMMAGE_COMMON_FILESEARCH_H_
 
-#include <unordered_map>
 #include <list>
 #include <memory>
 #include <string>
+#include <unordered_map>
 
 namespace boost {
-template <class T> class optional;
+template <class T>
+class optional;
 }
 
 namespace scrimmage {
@@ -57,20 +58,21 @@ class FileSearch {
      */
     boost::optional<std::string> find_mission(std::string mission, bool verbose = false);
 
-    bool find_file(const std::string &filename, std::string ext,
-        const std::string &env_var, std::string &result, bool verbose = false);
-    void find_files(std::string env_var, const std::string &ext,
-        std::unordered_map<std::string, std::list<std::string>> &out,
-        bool verbose = false);
+    bool find_file(const std::string& filename, std::string ext, const std::string& env_var,
+                   std::string& result, bool verbose = false);
+    void find_files(std::string env_var, const std::string& ext,
+                    std::unordered_map<std::string, std::list<std::string>>& out,
+                    bool verbose = false);
 
  protected:
     // cache_[env_var][ext][filename] = list of full paths to files with that filename
-    std::unordered_map<std::string,
-        std::unordered_map<std::string,
-            std::unordered_map<std::string, std::list<std::string>>>> cache_;
+    std::unordered_map<
+        std::string,
+        std::unordered_map<std::string, std::unordered_map<std::string, std::list<std::string>>>>
+        cache_;
 };
 
 using FileSearchPtr = std::shared_ptr<FileSearch>;
-} // namespace scrimmage
+}  // namespace scrimmage
 
-#endif // INCLUDE_SCRIMMAGE_COMMON_FILESEARCH_H_
+#endif  // INCLUDE_SCRIMMAGE_COMMON_FILESEARCH_H_
diff --git a/include/scrimmage/common/GlobalService.h b/include/scrimmage/common/GlobalService.h
index b0ea7833b1..b470863fda 100644
--- a/include/scrimmage/common/GlobalService.h
+++ b/include/scrimmage/common/GlobalService.h
@@ -33,19 +33,20 @@
 #define INCLUDE_SCRIMMAGE_COMMON_GLOBALSERVICE_H_
 
 #include <scrimmage/pubsub/Message.h>
+
+#include <functional>
+#include <iostream>
+#include <list>
 #include <map>
+#include <memory>
 #include <set>
+#include <string>
 #include <unordered_map>
-#include <list>
 #include <vector>
-#include <string>
-#include <functional>
-#include <memory>
-#include <iostream>
 
 namespace scrimmage {
 
-using Service = std::function<bool (scrimmage::MessageBasePtr, scrimmage::MessageBasePtr&)>;
+using Service = std::function<bool(scrimmage::MessageBasePtr, scrimmage::MessageBasePtr&)>;
 
 class GlobalService {
  public:
@@ -54,22 +55,23 @@ class GlobalService {
 
     /////////////////////////////
     // All service call functions
-    std::unordered_map<std::string, Service> &services();
+    std::unordered_map<std::string, Service>& services();
 
-    bool call_service(MessageBasePtr req, MessageBasePtr &res, const std::string &service_name);
+    bool call_service(MessageBasePtr req, MessageBasePtr& res, const std::string& service_name);
 
-    bool call_service(MessageBasePtr &res, const std::string &service_name) {
+    bool call_service(MessageBasePtr& res, const std::string& service_name) {
         return call_service(std::make_shared<MessageBase>(), res, service_name);
     }
 
     template <class T = MessageBasePtr,
               class = typename std::enable_if<!std::is_same<T, MessageBasePtr>::value, void>::type>
-    bool call_service(MessageBasePtr req, T &res, const std::string &service_name) {
+    bool call_service(MessageBasePtr req, T& res, const std::string& service_name) {
         MessageBasePtr res_base;
         if (call_service(req, res_base, service_name)) {
             res = std::dynamic_pointer_cast<typename T::element_type>(res_base);
             if (res == nullptr) {
-                std::cout << "could not cast for global service " << service_name.c_str() << std::endl;
+                std::cout << "could not cast for global service " << service_name.c_str()
+                          << std::endl;
                 return false;
             } else {
                 return true;
@@ -81,7 +83,7 @@ class GlobalService {
 
     template <class T = MessageBasePtr,
               class = typename std::enable_if<!std::is_same<T, MessageBasePtr>::value, void>::type>
-    bool call_service(T &res, const std::string &service_name) {
+    bool call_service(T& res, const std::string& service_name) {
         return call_service(std::make_shared<MessageBase>(), res, service_name);
     }
 
diff --git a/include/scrimmage/common/ID.h b/include/scrimmage/common/ID.h
index 2dff5818eb..ef3745207b 100644
--- a/include/scrimmage/common/ID.h
+++ b/include/scrimmage/common/ID.h
@@ -50,8 +50,8 @@ class ID {
     int sub_swarm_id() const;
     int team_id() const;
 
-    bool operator==(const ID &other) const;
-    bool operator<(const ID &other) const;
+    bool operator==(const ID& other) const;
+    bool operator<(const ID& other) const;
 
     friend std::ostream& operator<<(std::ostream& os, const scrimmage::ID& id);
 
@@ -60,6 +60,6 @@ class ID {
     int sub_swarm_id_;
     int team_id_;
 };
-} // namespace scrimmage
+}  // namespace scrimmage
 
-#endif // INCLUDE_SCRIMMAGE_COMMON_ID_H_
+#endif  // INCLUDE_SCRIMMAGE_COMMON_ID_H_
diff --git a/include/scrimmage/common/PID.h b/include/scrimmage/common/PID.h
index d6e0701a93..fcfb1278c7 100644
--- a/include/scrimmage/common/PID.h
+++ b/include/scrimmage/common/PID.h
@@ -39,26 +39,25 @@ namespace scrimmage {
 class PID {
  public:
     PID();
-    void set_parameters(const double &kp, const double &ki, const double &kd);
-    void set_setpoint(const double &setpoint);
-    void set_integral_band(const double &integral_band);
-    void set_is_angle(const bool &is_angle);
-    void set_output_limits(const double &min, const double &max,
-                           const double &enable);
+    void set_parameters(const double& kp, const double& ki, const double& kd);
+    void set_setpoint(const double& setpoint);
+    void set_integral_band(const double& integral_band);
+    void set_is_angle(const bool& is_angle);
+    void set_output_limits(const double& min, const double& max, const double& enable);
     void reset();
 
-    const double &setpoint();
-    const double &kp();
-    const double &ki();
-    const double &kd();
-    const double &integral_band();
-    const double &integral();
-    const double &error();
-    const double &previous_error();
-    const double &derivative();
+    const double& setpoint();
+    const double& kp();
+    const double& ki();
+    const double& kd();
+    const double& integral_band();
+    const double& integral();
+    const double& error();
+    const double& previous_error();
+    const double& derivative();
 
-    bool init(const std::string &str, const bool &is_angle);
-    double step(const double &dt, const double &measurement);
+    bool init(const std::string& str, const bool& is_angle);
+    double step(const double& dt, const double& measurement);
 
  protected:
     double kp_;
@@ -79,6 +78,6 @@ class PID {
     double output_max_;
     bool enable_output_limits_;
 };
-} // namespace scrimmage
+}  // namespace scrimmage
 
-#endif // INCLUDE_SCRIMMAGE_COMMON_PID_H_
+#endif  // INCLUDE_SCRIMMAGE_COMMON_PID_H_
diff --git a/include/scrimmage/common/Parameter.h b/include/scrimmage/common/Parameter.h
index 1ff8535f69..c906a25eca 100644
--- a/include/scrimmage/common/Parameter.h
+++ b/include/scrimmage/common/Parameter.h
@@ -33,9 +33,9 @@
 #ifndef INCLUDE_SCRIMMAGE_COMMON_PARAMETER_H_
 #define INCLUDE_SCRIMMAGE_COMMON_PARAMETER_H_
 
-#include <string>
-#include <memory>
 #include <functional>
+#include <memory>
+#include <string>
 
 namespace scrimmage {
 
@@ -44,9 +44,14 @@ using PluginPtr = std::shared_ptr<Plugin>;
 
 class ParameterBase {
  public:
-    explicit ParameterBase(PluginPtr &owner) : owner_(owner) {}
-    virtual ~ParameterBase() {}
-    const PluginPtr &owner() { return owner_; }
+    explicit ParameterBase(PluginPtr& owner) : owner_(owner) {
+    }
+    virtual ~ParameterBase() {
+    }
+    const PluginPtr& owner() {
+        return owner_;
+    }
+
  protected:
     PluginPtr owner_ = nullptr;
 };
@@ -54,19 +59,20 @@ class ParameterBase {
 template <class T>
 class Parameter : public ParameterBase {
  public:
-    Parameter(T &variable, std::function<void(const T &value)> callback,
-              PluginPtr &owner) : ParameterBase(owner), value_(variable),
-        callback_(callback) {}
-    void set_value(const T &value) {
+    Parameter(T& variable, std::function<void(const T& value)> callback, PluginPtr& owner)
+        : ParameterBase(owner), value_(variable), callback_(callback) {
+    }
+    void set_value(const T& value) {
         value_ = value;
         callback_(value_);
     }
+
  protected:
-    T &value_;
-    std::function<void(const T &value)> callback_;
+    T& value_;
+    std::function<void(const T& value)> callback_;
 };
 
 typedef std::shared_ptr<ParameterBase> ParameterBasePtr;
 
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_COMMON_PARAMETER_H_
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_COMMON_PARAMETER_H_
diff --git a/include/scrimmage/common/ParameterServer.h b/include/scrimmage/common/ParameterServer.h
index cadc2b7aa8..4b47a32f26 100644
--- a/include/scrimmage/common/ParameterServer.h
+++ b/include/scrimmage/common/ParameterServer.h
@@ -35,16 +35,15 @@
 
 #include <scrimmage/common/Parameter.h>
 
-#include <string>
-#include <unordered_map>
-#include <set>
-#include <tuple>
-#include <memory>
-#include <iostream>
 #include <algorithm>
-#include <functional>
-
 #include <boost/optional.hpp>
+#include <functional>
+#include <iostream>
+#include <memory>
+#include <set>
+#include <string>
+#include <tuple>
+#include <unordered_map>
 
 class Plugin;
 using PluginPtr = std::shared_ptr<Plugin>;
@@ -55,16 +54,15 @@ class ParameterServer {
     void unregister_params(PluginPtr owner);
 
     template <class T>
-    bool register_param(const std::string &name, T &variable,
-                        std::function<void(const T &value)> callback,
-                        PluginPtr owner) {
+    bool register_param(const std::string& name, T& variable,
+                        std::function<void(const T& value)> callback, PluginPtr owner) {
         auto it = params_[name][typeid(T).name()].emplace(
             std::make_shared<Parameter<T>>(variable, callback, owner));
-        return it.second; // return false if the param already exists
+        return it.second;  // return false if the param already exists
     }
 
     template <class T>
-    bool set_param(const std::string &name, const T &value) {
+    bool set_param(const std::string& name, const T& value) {
         auto param_set = find_name_type(name, typeid(T).name());
         if (param_set) {
             for (ParameterBasePtr param : *param_set) {
@@ -77,7 +75,7 @@ class ParameterServer {
     }
 
     template <class T>
-    bool unregister_param(const std::string &name, PluginPtr owner) {
+    bool unregister_param(const std::string& name, PluginPtr owner) {
         auto param_set = find_name_type(name, typeid(T).name());
         if (param_set) {
             // Remove the parameter if this is the owner
@@ -88,19 +86,17 @@ class ParameterServer {
     }
 
  protected:
-    bool remove_if_owner(std::set<ParameterBasePtr> &param_set,
-                         PluginPtr owner);
+    bool remove_if_owner(std::set<ParameterBasePtr>& param_set, PluginPtr owner);
 
-    inline boost::optional<std::set<ParameterBasePtr>&>
-        find_name_type(const std::string &name, const std::string &type) {
+    inline boost::optional<std::set<ParameterBasePtr>&> find_name_type(const std::string& name,
+                                                                       const std::string& type) {
         // Search for the parameter name
         auto it_name = params_.find(name);
         if (it_name != params_.end()) {
             // Search for the parameter type
             auto it_type = it_name->second.find(type);
             if (it_type != it_name->second.end()) {
-                return boost::optional<std::set<ParameterBasePtr>&>
-                    (it_type->second);
+                return boost::optional<std::set<ParameterBasePtr>&>(it_type->second);
             }
         }
         return boost::none;
@@ -109,9 +105,9 @@ class ParameterServer {
     // Key 1: parameter name (string)
     // Key 2: parameter type (as a string)
     // Value: Set of ParameterBasePtr
-    std::unordered_map<std::string,
-        std::unordered_map<std::string, std::set<ParameterBasePtr>>> params_;
+    std::unordered_map<std::string, std::unordered_map<std::string, std::set<ParameterBasePtr>>>
+        params_;
 };
 using ParameterServerPtr = std::shared_ptr<ParameterServer>;
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_COMMON_PARAMETERSERVER_H_
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_COMMON_PARAMETERSERVER_H_
diff --git a/include/scrimmage/common/Random.h b/include/scrimmage/common/Random.h
index 0928cdd60c..51f5169390 100644
--- a/include/scrimmage/common/Random.h
+++ b/include/scrimmage/common/Random.h
@@ -52,15 +52,15 @@ class Random {
     double rng_uniform(double low, double high);
     double rng_normal();
     double rng_normal(double mean, double sigma);
-    int rng_uniform_int(int low, int high); // inclusive of low and high
+    int rng_uniform_int(int low, int high);  // inclusive of low and high
 
-    std::shared_ptr<std::normal_distribution<double>>
-        make_rng_normal(double mean, double sigma);
+    std::shared_ptr<std::normal_distribution<double>> make_rng_normal(double mean, double sigma);
 
-    int rng_discrete_int(std::vector<double> &weights);
+    int rng_discrete_int(std::vector<double>& weights);
 
-    std::shared_ptr<std::default_random_engine> gener()
-    { return gener_; }
+    std::shared_ptr<std::default_random_engine> gener() {
+        return gener_;
+    }
 
  protected:
     uint32_t seed_;
@@ -70,6 +70,6 @@ class Random {
 };
 
 typedef std::shared_ptr<Random> RandomPtr;
-} // namespace scrimmage
+}  // namespace scrimmage
 
-#endif // INCLUDE_SCRIMMAGE_COMMON_RANDOM_H_
+#endif  // INCLUDE_SCRIMMAGE_COMMON_RANDOM_H_
diff --git a/include/scrimmage/common/Shape.h b/include/scrimmage/common/Shape.h
index f12634700c..4c99bf2e74 100644
--- a/include/scrimmage/common/Shape.h
+++ b/include/scrimmage/common/Shape.h
@@ -33,129 +33,121 @@
 #ifndef INCLUDE_SCRIMMAGE_COMMON_SHAPE_H_
 #define INCLUDE_SCRIMMAGE_COMMON_SHAPE_H_
 
-#include <scrimmage/proto/Shape.pb.h>
-#include <scrimmage/math/State.h>
 #include <scrimmage/math/Quaternion.h>
+#include <scrimmage/math/State.h>
+#include <scrimmage/proto/Shape.pb.h>
 
 #include <Eigen/Dense>
-
+#include <list>
 #include <memory>
 #include <string>
-#include <list>
 
 namespace scrimmage {
 namespace shape {
 
 using ShapePtr = std::shared_ptr<scrimmage_proto::Shape>;
 
-ShapePtr make_shape(const Eigen::Vector3d &color = Eigen::Vector3d(0, 0, 255),
-                    const double &opacity = 1.0);
-
-ShapePtr make_arc(const Eigen::Vector3d &center = Eigen::Vector3d(0, 0, 0),
-                  const scrimmage::Quaternion &quat = scrimmage::Quaternion(0, 0, 0),
-                  const double &radius = 1.0,
-                  const double &angle = M_PI / 2.0,
-                  const Eigen::Vector3d &color = Eigen::Vector3d(0, 0, 255),
-                  const double &opacity = 1.0);
-
-ShapePtr make_arrow(const Eigen::Vector3d &pos = Eigen::Vector3d(0, 0, 0),
-                    const scrimmage::Quaternion &quat = scrimmage::Quaternion(0, 0, 0),
-                    const double &length = 10.0,
-                    const Eigen::Vector3d &color = Eigen::Vector3d(0, 0, 255),
-                    const double &opacity = 1.0);
-
-ShapePtr make_circle(const Eigen::Vector3d &center = Eigen::Vector3d(0, 0, 0),
-                     const scrimmage::Quaternion &quat = scrimmage::Quaternion(0, 0, 0),
-                     const double &radius = 1.0,
-                     const Eigen::Vector3d &color = Eigen::Vector3d(0, 0, 255),
-                     const double &opacity = 1.0);
-
-ShapePtr make_cone(const Eigen::Vector3d &direction = Eigen::Vector3d(0, 0, 1),
-                   const Eigen::Vector3d &apex = Eigen::Vector3d(0, 0, 1),
-                   const double &height = 1.0,
-                   const double &base_radius = 1.0,
-                   const Eigen::Vector3d &color = Eigen::Vector3d(0, 0, 255),
-                   const double &opacity = 1.0);
-
-ShapePtr make_cuboid(const Eigen::Vector3d &center = Eigen::Vector3d(0, 0, 0),
-                     const scrimmage::Quaternion &quat = scrimmage::Quaternion(0, 0, 0),
-                     const double &x_length = 1.0,
-                     const double &y_length = 1.0,
-                     const double &z_length = 1.0,
-                     const Eigen::Vector3d &color = Eigen::Vector3d(0, 0, 255),
-                     const double &opacity = 1.0);
-
-ShapePtr make_ellipse(const Eigen::Vector3d &center = Eigen::Vector3d(0, 0, 0),
-                      const scrimmage::Quaternion &quat = scrimmage::Quaternion(0, 0, 0),
-                      const double &x_radius = 2.0,
-                      const double &y_radius = 1.0,
-                      const Eigen::Vector3d &color = Eigen::Vector3d(0, 0, 255),
-                      const double &opacity = 1.0);
-
-ShapePtr make_line(const Eigen::Vector3d &start = Eigen::Vector3d(0, 0, 0),
-                   const Eigen::Vector3d &stop = Eigen::Vector3d(1, 0, 0),
-                   const Eigen::Vector3d &color = Eigen::Vector3d(0, 0, 255),
-                   const double &opacity = 1.0);
-
-ShapePtr make_mesh(const std::string &name = "volkswagen",
-                   const Eigen::Vector3d &center = Eigen::Vector3d(0, 0, 0),
-                   const scrimmage::Quaternion &quat = scrimmage::Quaternion(0, 0, 0),
-                   const double &scale = 1.0,
-                   const Eigen::Vector3d &color = Eigen::Vector3d(0, 0, 255),
-                   const double &opacity = 1.0);
-
-ShapePtr make_plane(const Eigen::Vector3d &center = Eigen::Vector3d(0, 0, 0),
-                    const scrimmage::Quaternion &quat = scrimmage::Quaternion(0, 0, 0),
-                    const double &x_length = 10.0,
-                    const double &y_length = 10.0,
-                    const std::string &texture = "",
-                    const bool &diffuse_lighting = false,
-                    const Eigen::Vector3d &color = Eigen::Vector3d(0, 0, 255),
-                    const double &opacity = 1.0);
-
-ShapePtr make_pointcloud(const std::list<Eigen::Vector3d> &points,
-                         const std::list<Eigen::Vector3d> &point_colors,
-                         const double &size = 1.0,
-                         const Eigen::Vector3d &color = Eigen::Vector3d(0, 0, 255),
-                         const double &opacity = 1.0);
-
-ShapePtr make_polydata(const std::list<Eigen::Vector3d> &points,
-                       const Eigen::Vector3d &color = Eigen::Vector3d(0, 0, 255),
-                       const double &opacity = 1.0);
-
-ShapePtr make_polygon(const std::list<Eigen::Vector3d> &points,
-                      const Eigen::Vector3d &color = Eigen::Vector3d(0, 0, 255),
-                      const double &opacity = 1.0);
-
-ShapePtr make_polyhedron(const std::list<Eigen::Vector3d> &points,
-                         const Eigen::Vector3d &color = Eigen::Vector3d(0, 0, 255),
-                         const double &opacity = 1.0);
-
-ShapePtr make_polyline(const std::list<Eigen::Vector3d> &points,
-                       const Eigen::Vector3d &color = Eigen::Vector3d(0, 0, 255),
-                       const double &opacity = 1.0);
-
-ShapePtr make_sphere(const Eigen::Vector3d &center = Eigen::Vector3d(0, 0, 0),
-                     const double &radius = 1.0,
-                     const Eigen::Vector3d &color = Eigen::Vector3d(0, 0, 255),
-                     const double &opacity = 1.0);
-
-ShapePtr make_spline(const std::list<Eigen::Vector3d> &points,
-                     const Eigen::Vector3d &color = Eigen::Vector3d(0, 0, 255),
-                     const double &opacity = 1.0);
-
-ShapePtr make_text(const std::string &text,
-                   const Eigen::Vector3d &center = Eigen::Vector3d(0, 0, 0),
-                   const Eigen::Vector3d &color = Eigen::Vector3d(0, 0, 255),
-                   const double &opacity = 1.0);
-
-ShapePtr make_triangle(const Eigen::Vector3d &point0 = Eigen::Vector3d(1, 0, 0),
-                       const Eigen::Vector3d &point1 = Eigen::Vector3d(0, 1, 0),
-                       const Eigen::Vector3d &point2 = Eigen::Vector3d(-1, 0, 0),
-                       const Eigen::Vector3d &color = Eigen::Vector3d(0, 0, 255),
-                       const double &opacity = 1.0);
-
-} // namespace shape
-} // namespace scrimmage
-
-#endif // INCLUDE_SCRIMMAGE_COMMON_SHAPE_H_
+ShapePtr make_shape(const Eigen::Vector3d& color = Eigen::Vector3d(0, 0, 255),
+                    const double& opacity = 1.0);
+
+ShapePtr make_arc(const Eigen::Vector3d& center = Eigen::Vector3d(0, 0, 0),
+                  const scrimmage::Quaternion& quat = scrimmage::Quaternion(0, 0, 0),
+                  const double& radius = 1.0, const double& angle = M_PI / 2.0,
+                  const Eigen::Vector3d& color = Eigen::Vector3d(0, 0, 255),
+                  const double& opacity = 1.0);
+
+ShapePtr make_arrow(const Eigen::Vector3d& pos = Eigen::Vector3d(0, 0, 0),
+                    const scrimmage::Quaternion& quat = scrimmage::Quaternion(0, 0, 0),
+                    const double& length = 10.0,
+                    const Eigen::Vector3d& color = Eigen::Vector3d(0, 0, 255),
+                    const double& opacity = 1.0);
+
+ShapePtr make_circle(const Eigen::Vector3d& center = Eigen::Vector3d(0, 0, 0),
+                     const scrimmage::Quaternion& quat = scrimmage::Quaternion(0, 0, 0),
+                     const double& radius = 1.0,
+                     const Eigen::Vector3d& color = Eigen::Vector3d(0, 0, 255),
+                     const double& opacity = 1.0);
+
+ShapePtr make_cone(const Eigen::Vector3d& direction = Eigen::Vector3d(0, 0, 1),
+                   const Eigen::Vector3d& apex = Eigen::Vector3d(0, 0, 1),
+                   const double& height = 1.0, const double& base_radius = 1.0,
+                   const Eigen::Vector3d& color = Eigen::Vector3d(0, 0, 255),
+                   const double& opacity = 1.0);
+
+ShapePtr make_cuboid(const Eigen::Vector3d& center = Eigen::Vector3d(0, 0, 0),
+                     const scrimmage::Quaternion& quat = scrimmage::Quaternion(0, 0, 0),
+                     const double& x_length = 1.0, const double& y_length = 1.0,
+                     const double& z_length = 1.0,
+                     const Eigen::Vector3d& color = Eigen::Vector3d(0, 0, 255),
+                     const double& opacity = 1.0);
+
+ShapePtr make_ellipse(const Eigen::Vector3d& center = Eigen::Vector3d(0, 0, 0),
+                      const scrimmage::Quaternion& quat = scrimmage::Quaternion(0, 0, 0),
+                      const double& x_radius = 2.0, const double& y_radius = 1.0,
+                      const Eigen::Vector3d& color = Eigen::Vector3d(0, 0, 255),
+                      const double& opacity = 1.0);
+
+ShapePtr make_line(const Eigen::Vector3d& start = Eigen::Vector3d(0, 0, 0),
+                   const Eigen::Vector3d& stop = Eigen::Vector3d(1, 0, 0),
+                   const Eigen::Vector3d& color = Eigen::Vector3d(0, 0, 255),
+                   const double& opacity = 1.0);
+
+ShapePtr make_mesh(const std::string& name = "volkswagen",
+                   const Eigen::Vector3d& center = Eigen::Vector3d(0, 0, 0),
+                   const scrimmage::Quaternion& quat = scrimmage::Quaternion(0, 0, 0),
+                   const double& scale = 1.0,
+                   const Eigen::Vector3d& color = Eigen::Vector3d(0, 0, 255),
+                   const double& opacity = 1.0);
+
+ShapePtr make_plane(const Eigen::Vector3d& center = Eigen::Vector3d(0, 0, 0),
+                    const scrimmage::Quaternion& quat = scrimmage::Quaternion(0, 0, 0),
+                    const double& x_length = 10.0, const double& y_length = 10.0,
+                    const std::string& texture = "", const bool& diffuse_lighting = false,
+                    const Eigen::Vector3d& color = Eigen::Vector3d(0, 0, 255),
+                    const double& opacity = 1.0);
+
+ShapePtr make_pointcloud(const std::list<Eigen::Vector3d>& points,
+                         const std::list<Eigen::Vector3d>& point_colors, const double& size = 1.0,
+                         const Eigen::Vector3d& color = Eigen::Vector3d(0, 0, 255),
+                         const double& opacity = 1.0);
+
+ShapePtr make_polydata(const std::list<Eigen::Vector3d>& points,
+                       const Eigen::Vector3d& color = Eigen::Vector3d(0, 0, 255),
+                       const double& opacity = 1.0);
+
+ShapePtr make_polygon(const std::list<Eigen::Vector3d>& points,
+                      const Eigen::Vector3d& color = Eigen::Vector3d(0, 0, 255),
+                      const double& opacity = 1.0);
+
+ShapePtr make_polyhedron(const std::list<Eigen::Vector3d>& points,
+                         const Eigen::Vector3d& color = Eigen::Vector3d(0, 0, 255),
+                         const double& opacity = 1.0);
+
+ShapePtr make_polyline(const std::list<Eigen::Vector3d>& points,
+                       const Eigen::Vector3d& color = Eigen::Vector3d(0, 0, 255),
+                       const double& opacity = 1.0);
+
+ShapePtr make_sphere(const Eigen::Vector3d& center = Eigen::Vector3d(0, 0, 0),
+                     const double& radius = 1.0,
+                     const Eigen::Vector3d& color = Eigen::Vector3d(0, 0, 255),
+                     const double& opacity = 1.0);
+
+ShapePtr make_spline(const std::list<Eigen::Vector3d>& points,
+                     const Eigen::Vector3d& color = Eigen::Vector3d(0, 0, 255),
+                     const double& opacity = 1.0);
+
+ShapePtr make_text(const std::string& text,
+                   const Eigen::Vector3d& center = Eigen::Vector3d(0, 0, 0),
+                   const Eigen::Vector3d& color = Eigen::Vector3d(0, 0, 255),
+                   const double& opacity = 1.0);
+
+ShapePtr make_triangle(const Eigen::Vector3d& point0 = Eigen::Vector3d(1, 0, 0),
+                       const Eigen::Vector3d& point1 = Eigen::Vector3d(0, 1, 0),
+                       const Eigen::Vector3d& point2 = Eigen::Vector3d(-1, 0, 0),
+                       const Eigen::Vector3d& color = Eigen::Vector3d(0, 0, 255),
+                       const double& opacity = 1.0);
+
+}  // namespace shape
+}  // namespace scrimmage
+
+#endif  // INCLUDE_SCRIMMAGE_COMMON_SHAPE_H_
diff --git a/include/scrimmage/common/Time.h b/include/scrimmage/common/Time.h
index f608ee5ffb..c022001484 100644
--- a/include/scrimmage/common/Time.h
+++ b/include/scrimmage/common/Time.h
@@ -68,5 +68,5 @@ class Time {
     double time_warp_;
 };
 using TimePtr = std::shared_ptr<Time>;
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_COMMON_TIME_H_
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_COMMON_TIME_H_
diff --git a/include/scrimmage/common/Timer.h b/include/scrimmage/common/Timer.h
index 52f094ef43..083326cf1c 100644
--- a/include/scrimmage/common/Timer.h
+++ b/include/scrimmage/common/Timer.h
@@ -79,5 +79,5 @@ class Timer {
     boost::posix_time::time_duration sim_time_period_;
     double iterate_rate_;
 };
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_COMMON_TIMER_H_
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_COMMON_TIMER_H_
diff --git a/include/scrimmage/common/Utilities.h b/include/scrimmage/common/Utilities.h
index 8015235d0e..0010d507af 100644
--- a/include/scrimmage/common/Utilities.h
+++ b/include/scrimmage/common/Utilities.h
@@ -34,42 +34,37 @@
 #define INCLUDE_SCRIMMAGE_COMMON_UTILITIES_H_
 
 #include <Eigen/Dense>
-
 #include <map>
-#include <vector>
 #include <string>
+#include <vector>
 
 namespace scrimmage {
 
 void display_progress(float progress);
 
-int next_available_id(std::string name,
-                      std::map<std::string, std::string> &info,
-                      std::map<int, int> &id_map);
+int next_available_id(std::string name, std::map<std::string, std::string>& info,
+                      std::map<int, int>& id_map);
 
-std::string get_sha(std::string &path);
+std::string get_sha(std::string& path);
 
 std::string get_version();
 
-void filter_line(int downsampling_factor,
-    int num_points,
-    std::vector<Eigen::Vector3d> &path,
-    std::vector<Eigen::Vector3d> &filtered_path);
+void filter_line(int downsampling_factor, int num_points, std::vector<Eigen::Vector3d>& path,
+                 std::vector<Eigen::Vector3d>& filtered_path);
 
-std::string generate_chars(const std::string &symbol, int num);
+std::string generate_chars(const std::string& symbol, int num);
 
-std::string eigen_str(const Eigen::VectorXd &vec, uint8_t precision = 2);
+std::string eigen_str(const Eigen::VectorXd& vec, uint8_t precision = 2);
 
 std::vector<double> linspace(double low, double high, uint32_t n);
 
 template <class T>
-T interp(const T &low, const T &high, double pct) {
+T interp(const T& low, const T& high, double pct) {
     return low + pct * (high - low);
 }
 
 template <class T>
-T scale(const T &input, const T &in_min, const T &in_max,
-        const T &out_min, const T &out_max) {
+T scale(const T& input, const T& in_min, const T& in_max, const T& out_min, const T& out_max) {
     T result = input;
     // Check for input/output min/max bounds
     if (in_min > in_max || out_min > out_max) {
@@ -89,8 +84,8 @@ T scale(const T &input, const T &in_min, const T &in_max,
 }
 
 template <class T>
-Eigen::VectorXd scale(const Eigen::VectorXd &input, const T &in_min,
-                      const T &in_max, const T &out_min, const T &out_max) {
+Eigen::VectorXd scale(const Eigen::VectorXd& input, const T& in_min, const T& in_max,
+                      const T& out_min, const T& out_max) {
     Eigen::VectorXd result = input;
     for (int i = 0; i < input.size(); i++) {
         result(i) = scale(input(i), in_min, in_max, out_min, out_max);
@@ -98,6 +93,6 @@ Eigen::VectorXd scale(const Eigen::VectorXd &input, const T &in_min,
     return result;
 }
 
-} // namespace scrimmage
+}  // namespace scrimmage
 
-#endif // INCLUDE_SCRIMMAGE_COMMON_UTILITIES_H_
+#endif  // INCLUDE_SCRIMMAGE_COMMON_UTILITIES_H_
diff --git a/include/scrimmage/common/VariableIO.h b/include/scrimmage/common/VariableIO.h
index 9928c648d3..1b59bf6334 100644
--- a/include/scrimmage/common/VariableIO.h
+++ b/include/scrimmage/common/VariableIO.h
@@ -34,11 +34,10 @@
 #define INCLUDE_SCRIMMAGE_COMMON_VARIABLEIO_H_
 
 #include <Eigen/Dense>
-
-#include <set>
 #include <map>
-#include <string>
 #include <memory>
+#include <set>
+#include <string>
 
 namespace scrimmage {
 /*! \brief abstracts the connection between motion models, controllers, and autonomies
@@ -46,7 +45,7 @@ namespace scrimmage {
 class VariableIO;
 class VariableIO {
  public:
-    enum class Direction {In = 0, Out};
+    enum class Direction { In = 0, Out };
     enum class Type {
         desired_altitude,
         desired_speed,
@@ -78,11 +77,11 @@ class VariableIO {
 
     VariableIO();
 
-    std::map<std::string, int> &output_variable_index();
-    std::map<std::string, int> &input_variable_index();
+    std::map<std::string, int>& output_variable_index();
+    std::map<std::string, int>& input_variable_index();
 
-    int add_input_variable(const std::string &var);
-    int add_output_variable(const std::string &var);
+    int add_input_variable(const std::string& var);
+    int add_output_variable(const std::string& var);
     int declare(std::string var, Direction dir);
     int declare(Type type, Direction dir);
 
@@ -96,17 +95,23 @@ class VariableIO {
     std::set<std::string> declared_input_variables();
     std::set<std::string> declared_output_variables();
 
-    const std::map<Type, std::string> &type_map() const { return type_map_; }
+    const std::map<Type, std::string>& type_map() const {
+        return type_map_;
+    }
 
-    const std::shared_ptr<Eigen::VectorXd> &input() { return input_; }
-    const std::shared_ptr<Eigen::VectorXd> &output() { return output_; }
-    void set_input(const std::shared_ptr<Eigen::VectorXd> &input);
-    void set_output(const std::shared_ptr<Eigen::VectorXd> &output);
+    const std::shared_ptr<Eigen::VectorXd>& input() {
+        return input_;
+    }
+    const std::shared_ptr<Eigen::VectorXd>& output() {
+        return output_;
+    }
+    void set_input(const std::shared_ptr<Eigen::VectorXd>& input);
+    void set_output(const std::shared_ptr<Eigen::VectorXd>& output);
 
     /*! \brief Connect two VariableIO objects where writing to the output of
      *  the first object will transfer the data to the input of the second
      *  object. */
-    friend void connect(VariableIO &output, VariableIO &input);
+    friend void connect(VariableIO& output, VariableIO& input);
 
  protected:
     int next_input_variable_index_ = 0;
@@ -119,9 +124,9 @@ class VariableIO {
     static std::map<Type, std::string> type_map_;
 };
 
-void print_io_error(const std::string &in_name, VariableIO &v);
-bool verify_io_connection(VariableIO &output_plugin, VariableIO &input_plugin);
+void print_io_error(const std::string& in_name, VariableIO& v);
+bool verify_io_connection(VariableIO& output_plugin, VariableIO& input_plugin);
 
-} // namespace scrimmage
+}  // namespace scrimmage
 
-#endif // INCLUDE_SCRIMMAGE_COMMON_VARIABLEIO_H_
+#endif  // INCLUDE_SCRIMMAGE_COMMON_VARIABLEIO_H_
diff --git a/include/scrimmage/common/Visibility.h b/include/scrimmage/common/Visibility.h
index 539b60c351..15665da497 100644
--- a/include/scrimmage/common/Visibility.h
+++ b/include/scrimmage/common/Visibility.h
@@ -35,28 +35,28 @@
 
 // https://gcc.gnu.org/wiki/Visibility
 #if defined _WIN32 || defined __CYGWIN__
-  #ifdef BUILDING_DLL
-    #ifdef __GNUC__
-      #define DLL_PUBLIC __attribute__ ((dllexport))
-    #else
-      #define DLL_PUBLIC __declspec(dllexport) // Note: actually gcc seems to also supports this syntax.
-    #endif
-  #else
-    #ifdef __GNUC__
-      #define DLL_PUBLIC __attribute__ ((dllimport))
-    #else
-      #define DLL_PUBLIC __declspec(dllimport) // Note: actually gcc seems to also supports this syntax.
-    #endif
-  #endif
-  #define DLL_LOCAL
+#ifdef BUILDING_DLL
+#ifdef __GNUC__
+#define DLL_PUBLIC __attribute__((dllexport))
 #else
-  #if __GNUC__ >= 4
-    #define DLL_PUBLIC __attribute__ ((visibility ("default")))
-    #define DLL_LOCAL  __attribute__ ((visibility ("hidden")))
-  #else
-    #define DLL_PUBLIC
-    #define DLL_LOCAL
-  #endif
+#define DLL_PUBLIC __declspec(dllexport)  // Note: actually gcc seems to also supports this syntax.
+#endif
+#else
+#ifdef __GNUC__
+#define DLL_PUBLIC __attribute__((dllimport))
+#else
+#define DLL_PUBLIC __declspec(dllimport)  // Note: actually gcc seems to also supports this syntax.
+#endif
+#endif
+#define DLL_LOCAL
+#else
+#if __GNUC__ >= 4
+#define DLL_PUBLIC __attribute__((visibility("default")))
+#define DLL_LOCAL __attribute__((visibility("hidden")))
+#else
+#define DLL_PUBLIC
+#define DLL_LOCAL
+#endif
 #endif
 
-#endif // INCLUDE_SCRIMMAGE_COMMON_VISIBILITY_H_
+#endif  // INCLUDE_SCRIMMAGE_COMMON_VISIBILITY_H_
diff --git a/include/scrimmage/common/Waypoint.h b/include/scrimmage/common/Waypoint.h
index 48599766b0..5c167d05b1 100644
--- a/include/scrimmage/common/Waypoint.h
+++ b/include/scrimmage/common/Waypoint.h
@@ -36,9 +36,9 @@
 #include <scrimmage/math/State.h>
 #include <scrimmage/msgs/Waypoint.pb.h>
 
-#include <string>
 #include <map>
 #include <memory>
+#include <string>
 
 namespace GeographicLib {
 class LocalCartesian;
@@ -50,44 +50,69 @@ namespace autonomy {
 class Waypoint {
  public:
     Waypoint() = default;
-    Waypoint(const double& latitude, const double& longitude,
-             const double& altitude);
-    Waypoint(const double &x, const double& y, const double& z,
-             const std::shared_ptr<GeographicLib::LocalCartesian> &proj);
-    Waypoint(const Eigen::Vector3d &xyz,
-             const std::shared_ptr<GeographicLib::LocalCartesian> &proj);
-    explicit Waypoint(const scrimmage_msgs::Waypoint &wp);
-
-    void set_id(const size_t& id) {id_ = id;}
-    void set_time(const double& time) { time_ = time; }
-    void set_latitude(const double& latitude) { latitude_ = latitude; }
-    void set_longitude(const double& longitude) { longitude_ = longitude; }
-    void set_altitude(const double& altitude) { altitude_ = altitude; }
-    void set_quat(const scrimmage::Quaternion& quat) { quat_ = quat; }
+    Waypoint(const double& latitude, const double& longitude, const double& altitude);
+    Waypoint(const double& x, const double& y, const double& z,
+             const std::shared_ptr<GeographicLib::LocalCartesian>& proj);
+    Waypoint(const Eigen::Vector3d& xyz,
+             const std::shared_ptr<GeographicLib::LocalCartesian>& proj);
+    explicit Waypoint(const scrimmage_msgs::Waypoint& wp);
+
+    void set_id(const size_t& id) {
+        id_ = id;
+    }
+    void set_time(const double& time) {
+        time_ = time;
+    }
+    void set_latitude(const double& latitude) {
+        latitude_ = latitude;
+    }
+    void set_longitude(const double& longitude) {
+        longitude_ = longitude;
+    }
+    void set_altitude(const double& altitude) {
+        altitude_ = altitude;
+    }
+    void set_quat(const scrimmage::Quaternion& quat) {
+        quat_ = quat;
+    }
     void set_position_tolerance(const double& pos_tol);
     void set_quat_tolerance(const double& quat_tol);
 
     scrimmage_msgs::Waypoint lla_proto();
 
-    const size_t& id() const { return id_; }
-    const double& time() const { return time_; }
-    const double& latitude() const { return latitude_; }
-    const double& longitude() const { return longitude_; }
-    const double& altitude() const { return altitude_; }
-    const scrimmage::Quaternion &quat() const { return quat_; }
-    const double& position_tolerance() const { return position_tolerance_; }
-    const double& quat_tolerance() const { return quat_tolerance_; }
+    const size_t& id() const {
+        return id_;
+    }
+    const double& time() const {
+        return time_;
+    }
+    const double& latitude() const {
+        return latitude_;
+    }
+    const double& longitude() const {
+        return longitude_;
+    }
+    const double& altitude() const {
+        return altitude_;
+    }
+    const scrimmage::Quaternion& quat() const {
+        return quat_;
+    }
+    const double& position_tolerance() const {
+        return position_tolerance_;
+    }
+    const double& quat_tolerance() const {
+        return quat_tolerance_;
+    }
 
     friend std::ostream& operator<<(std::ostream& os, Waypoint& wp);
 
-    friend bool operator==(const Waypoint &lhs, const Waypoint &rhs);
+    friend bool operator==(const Waypoint& lhs, const Waypoint& rhs);
 
-    Eigen::Vector3d xyz(
-        const std::shared_ptr<GeographicLib::LocalCartesian> &proj) const;
+    Eigen::Vector3d xyz(const std::shared_ptr<GeographicLib::LocalCartesian>& proj) const;
 
-    bool is_within_tolerance(
-        const scrimmage::StatePtr &state,
-        const std::shared_ptr<GeographicLib::LocalCartesian> &proj);
+    bool is_within_tolerance(const scrimmage::StatePtr& state,
+                             const std::shared_ptr<GeographicLib::LocalCartesian>& proj);
 
  protected:
     size_t id_ = 0;
@@ -102,6 +127,6 @@ class Waypoint {
 
     bool tolerance_in_2d_ = false;
 };
-} // namespace autonomy
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_COMMON_WAYPOINT_H_
+}  // namespace autonomy
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_COMMON_WAYPOINT_H_
diff --git a/include/scrimmage/common/WaypointListProcessor.h b/include/scrimmage/common/WaypointListProcessor.h
index aedd3379a0..3a6b1a7519 100644
--- a/include/scrimmage/common/WaypointListProcessor.h
+++ b/include/scrimmage/common/WaypointListProcessor.h
@@ -33,24 +33,23 @@
 #ifndef INCLUDE_SCRIMMAGE_COMMON_WAYPOINTLISTPROCESSOR_H_
 #define INCLUDE_SCRIMMAGE_COMMON_WAYPOINTLISTPROCESSOR_H_
 
-#include <scrimmage/msgs/Waypoint.pb.h>
 #include <scrimmage/common/Waypoint.h>
 #include <scrimmage/math/State.h>
+#include <scrimmage/msgs/Waypoint.pb.h>
 
+#include <boost/optional.hpp>
 #include <list>
 #include <memory>
 
-#include <boost/optional.hpp>
-
 namespace scrimmage {
 
 namespace autonomy {
 class WaypointListProcessor {
  public:
-    enum class Status { Empty, Invalid, Changed, Unchanged  };
-    void set_waypoint_list(const scrimmage_msgs::WaypointList &wp_list);
-    Status process(const scrimmage::StatePtr &state,
-                   const std::shared_ptr<GeographicLib::LocalCartesian> &proj);
+    enum class Status { Empty, Invalid, Changed, Unchanged };
+    void set_waypoint_list(const scrimmage_msgs::WaypointList& wp_list);
+    Status process(const scrimmage::StatePtr& state,
+                   const std::shared_ptr<GeographicLib::LocalCartesian>& proj);
 
     boost::optional<const Waypoint&> previous_waypoint();
     boost::optional<const Waypoint&> current_waypoint();
@@ -67,9 +66,8 @@ class WaypointListProcessor {
     scrimmage_msgs::WaypointList::Mode mode_;
     bool returning_stage_ = false;
 
-    std::list<Waypoint>::iterator
-    next_waypoint(const std::list<Waypoint>::iterator &it_wp);
+    std::list<Waypoint>::iterator next_waypoint(const std::list<Waypoint>::iterator& it_wp);
 };
-} // namespace autonomy
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_COMMON_WAYPOINTLISTPROCESSOR_H_
+}  // namespace autonomy
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_COMMON_WAYPOINTLISTPROCESSOR_H_
diff --git a/include/scrimmage/entity/Contact.h b/include/scrimmage/entity/Contact.h
index f6b37ee131..7cf7484642 100644
--- a/include/scrimmage/entity/Contact.h
+++ b/include/scrimmage/entity/Contact.h
@@ -36,15 +36,15 @@
 #include <scrimmage/common/ID.h>
 #include <scrimmage/pubsub/Message.h>
 
+#include <iosfwd>
 #include <memory>
-#include <unordered_map>
 #include <string>
-#include <iosfwd>
+#include <unordered_map>
 
 namespace scrimmage_proto {
 class ContactVisual;
 using ContactVisualPtr = std::shared_ptr<scrimmage_proto::ContactVisual>;
-}
+}  // namespace scrimmage_proto
 
 namespace scrimmage {
 
@@ -53,22 +53,20 @@ using StatePtr = std::shared_ptr<State>;
 
 class Contact {
  public:
-    enum class Type {AIRCRAFT = 0, QUADROTOR, SPHERE, MESH, UNKNOWN};
+    enum class Type { AIRCRAFT = 0, QUADROTOR, SPHERE, MESH, UNKNOWN };
 
     Contact();
 
-    Contact(const ID &id, const StatePtr &state);
-    Contact(ID &id, double radius, StatePtr &state, Type type,
-        scrimmage_proto::ContactVisualPtr cv,
-        const std::unordered_map<std::string, MessageBasePtr> &properties);
+    Contact(const ID& id, const StatePtr& state);
+    Contact(ID& id, double radius, StatePtr& state, Type type, scrimmage_proto::ContactVisualPtr cv,
+            const std::unordered_map<std::string, MessageBasePtr>& properties);
 
     template <class T>
     std::unordered_map<std::string, MessagePtr<T>> get_properties(std::string name = "") {
         std::unordered_map<std::string, MessagePtr<T>> out;
-        for (auto &kv : properties_) {
+        for (auto& kv : properties_) {
             if (name == "" || kv.first.find(name) != std::string::npos) {
-                auto property_cast =
-                    std::dynamic_pointer_cast<Message<T>>(kv.second);
+                auto property_cast = std::dynamic_pointer_cast<Message<T>>(kv.second);
                 if (property_cast) {
                     out[kv.first] = property_cast;
                 }
@@ -77,24 +75,28 @@ class Contact {
         return out;
     }
 
-    std::unordered_map<std::string, MessageBasePtr> &properties() {return properties_;}
-    void set_id(const ID &id);
-    ID &id();
+    std::unordered_map<std::string, MessageBasePtr>& properties() {
+        return properties_;
+    }
+    void set_id(const ID& id);
+    ID& id();
 
-    void set_state(StatePtr &state);
-    StatePtr &state();
+    void set_state(StatePtr& state);
+    StatePtr& state();
     std::shared_ptr<const State> state_const() const;
 
     void set_type(Type type);
     Type type();
 
-    scrimmage_proto::ContactVisualPtr & contact_visual();
+    scrimmage_proto::ContactVisualPtr& contact_visual();
 
     void set_active(bool active);
     bool active();
 
     void set_radius(double radius);
-    double radius() { return radius_; }
+    double radius() {
+        return radius_;
+    }
 
     friend std::ostream& operator<<(std::ostream& os, const Contact& c);
 
@@ -110,5 +112,5 @@ class Contact {
 
 using ContactMap = std::unordered_map<int, Contact>;
 using ContactMapPtr = std::shared_ptr<ContactMap>;
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_ENTITY_CONTACT_H_
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_ENTITY_CONTACT_H_
diff --git a/include/scrimmage/entity/Entity.h b/include/scrimmage/entity/Entity.h
index caa37a9b90..223e3b6657 100644
--- a/include/scrimmage/entity/Entity.h
+++ b/include/scrimmage/entity/Entity.h
@@ -33,22 +33,21 @@
 #ifndef INCLUDE_SCRIMMAGE_ENTITY_ENTITY_H_
 #define INCLUDE_SCRIMMAGE_ENTITY_ENTITY_H_
 
-#include <scrimmage/fwd_decl.h>
 #include <scrimmage/common/ID.h>
 #include <scrimmage/entity/Contact.h>
+#include <scrimmage/fwd_decl.h>
 #include <scrimmage/proto/Visual.pb.h>
 #include <scrimmage/pubsub/Message.h>
 
+#include <boost/optional.hpp>
+#include <functional>
+#include <list>
 #include <map>
+#include <memory>
 #include <set>
+#include <string>
 #include <unordered_map>
-#include <list>
 #include <vector>
-#include <string>
-#include <functional>
-#include <memory>
-
-#include <boost/optional.hpp>
 
 namespace scrimmage_proto {
 using ContactVisualPtr = std::shared_ptr<ContactVisual>;
@@ -56,7 +55,7 @@ using ContactVisualPtr = std::shared_ptr<ContactVisual>;
 
 namespace scrimmage {
 
-using Service = std::function<bool (scrimmage::MessageBasePtr, scrimmage::MessageBasePtr&)>;
+using Service = std::function<bool(scrimmage::MessageBasePtr, scrimmage::MessageBasePtr&)>;
 
 typedef std::map<std::string, std::map<std::string, std::string>> AttributeMap;
 
@@ -65,31 +64,22 @@ class Entity : public std::enable_shared_from_this<Entity> {
     /*! \name utilities */
     ///@{
 
-    bool init(AttributeMap &overrides,
-              std::map<std::string, std::string> &info,
-              std::shared_ptr<std::unordered_map<int, int>> &id_to_team_map,
-              std::shared_ptr<std::unordered_map<int, EntityPtr>> &id_to_ent_map,
-              ContactMapPtr &contacts,
-              MissionParsePtr mp,
-              const std::shared_ptr<GeographicLib::LocalCartesian> &proj,
-              int id, int ent_desc_id,
-              PluginManagerPtr plugin_manager,
-              FileSearchPtr &file_search,
-              RTreePtr &rtree,
-              PubSubPtr &pubsub,
-              PrintPtr &printer,
-              TimePtr &time,
-              const ParameterServerPtr &param_server,
-              const GlobalServicePtr &global_services,
-              const std::set<std::string> &plugin_tags,
+    bool init(AttributeMap& overrides, std::map<std::string, std::string>& info,
+              std::shared_ptr<std::unordered_map<int, int>>& id_to_team_map,
+              std::shared_ptr<std::unordered_map<int, EntityPtr>>& id_to_ent_map,
+              ContactMapPtr& contacts, MissionParsePtr mp,
+              const std::shared_ptr<GeographicLib::LocalCartesian>& proj, int id, int ent_desc_id,
+              PluginManagerPtr plugin_manager, FileSearchPtr& file_search, RTreePtr& rtree,
+              PubSubPtr& pubsub, PrintPtr& printer, TimePtr& time,
+              const ParameterServerPtr& param_server, const GlobalServicePtr& global_services,
+              const std::set<std::string>& plugin_tags,
               std::function<void(std::map<std::string, std::string>&)> param_override_func,
               const int& debug_level = 0);
 
-    void print_plugins(std::ostream &out) const;
+    void print_plugins(std::ostream& out) const;
 
-    bool parse_visual(std::map<std::string, std::string> &info,
-                      MissionParsePtr mp,
-                      std::map<std::string, std::string> &overrides);
+    bool parse_visual(std::map<std::string, std::string>& info, MissionParsePtr mp,
+                      std::map<std::string, std::string>& overrides);
 
     void close(double t);
     void collision();
@@ -100,15 +90,15 @@ class Entity : public std::enable_shared_from_this<Entity> {
     void setup_desired_state();
     bool ready();
 
-    bool call_service(MessageBasePtr req, MessageBasePtr &res, const std::string &service_name);
+    bool call_service(MessageBasePtr req, MessageBasePtr& res, const std::string& service_name);
 
-    bool call_service(MessageBasePtr &res, const std::string &service_name) {
+    bool call_service(MessageBasePtr& res, const std::string& service_name) {
         return call_service(std::make_shared<MessageBase>(), res, service_name);
     }
 
     template <class T = MessageBasePtr,
               class = typename std::enable_if<!std::is_same<T, MessageBasePtr>::value, void>::type>
-    bool call_service(MessageBasePtr req, T &res, const std::string &service_name) {
+    bool call_service(MessageBasePtr req, T& res, const std::string& service_name) {
         MessageBasePtr res_base;
         if (call_service(req, res_base, service_name)) {
             res = std::dynamic_pointer_cast<typename T::element_type>(res_base);
@@ -125,27 +115,27 @@ class Entity : public std::enable_shared_from_this<Entity> {
 
     template <class T = MessageBasePtr,
               class = typename std::enable_if<!std::is_same<T, MessageBasePtr>::value, void>::type>
-    bool call_service(T &res, const std::string &service_name) {
+    bool call_service(T& res, const std::string& service_name) {
         return call_service(std::make_shared<MessageBase>(), res, service_name);
     }
     ///@}
 
     /*! \name getters/setters */
     ///@{
-    StatePtr &state();
-    StatePtr &state_truth();
-    std::vector<AutonomyPtr> &autonomies();
-    MotionModelPtr &motion();
-    std::vector<ControllerPtr> &controllers();
+    StatePtr& state();
+    StatePtr& state_truth();
+    std::vector<AutonomyPtr>& autonomies();
+    MotionModelPtr& motion();
+    std::vector<ControllerPtr>& controllers();
 
-    void set_id(ID &id);
-    ID &id();
+    void set_id(ID& id);
+    ID& id();
 
     void set_health_points(int health_points);
     int health_points();
 
     std::shared_ptr<GeographicLib::LocalCartesian> projection();
-    void set_projection(const std::shared_ptr<GeographicLib::LocalCartesian> &proj);
+    void set_projection(const std::shared_ptr<GeographicLib::LocalCartesian>& proj);
 
     void set_mp(MissionParsePtr mp);
     MissionParsePtr mp();
@@ -158,40 +148,44 @@ class Entity : public std::enable_shared_from_this<Entity> {
     void set_visual_changed(bool visual_changed);
     bool visual_changed();
 
-    scrimmage_proto::ContactVisualPtr &contact_visual();
+    scrimmage_proto::ContactVisualPtr& contact_visual();
 
-    std::unordered_map<std::string, SensorPtr> &sensors();
-    std::unordered_map<std::string, SensorPtr> sensors(const std::string &sensor_name);
-    SensorPtr sensor(const std::string &sensor_name);
+    std::unordered_map<std::string, SensorPtr>& sensors();
+    std::unordered_map<std::string, SensorPtr> sensors(const std::string& sensor_name);
+    SensorPtr sensor(const std::string& sensor_name);
 
     // Enables creating services at the entity level
-    std::unordered_map<std::string, Service> &services();
+    std::unordered_map<std::string, Service>& services();
     // Enables creating services at the global level (especially for entity interactions, etc.)
-    std::unordered_map<std::string, Service> &global_services();
+    std::unordered_map<std::string, Service>& global_services();
     // Enables setting these for entity interactions
-    void set_global_services(const GlobalServicePtr &global_services);
+    void set_global_services(const GlobalServicePtr& global_services);
 
-    std::unordered_map<std::string, MessageBasePtr> &properties();
+    std::unordered_map<std::string, MessageBasePtr>& properties();
 
     void set_active(bool active);
     bool active();
 
-    ContactMapPtr &contacts() { return contacts_; }
-    RTreePtr &rtree() { return rtree_; }
+    ContactMapPtr& contacts() {
+        return contacts_;
+    }
+    RTreePtr& rtree() {
+        return rtree_;
+    }
 
-    PluginManagerPtr & plugin_manager() {
+    PluginManagerPtr& plugin_manager() {
         return plugin_manager_;
     }
 
-    FileSearchPtr & file_search() {
+    FileSearchPtr& file_search() {
         return file_search_;
     }
 
-    PubSubPtr & pubsub() {
+    PubSubPtr& pubsub() {
         return pubsub_;
     }
 
-    PrintPtr & printer() {
+    PrintPtr& printer() {
         return printer_;
     }
 
@@ -199,7 +193,9 @@ class Entity : public std::enable_shared_from_this<Entity> {
         return param_server_;
     }
 
-    double radius() { return radius_; }
+    double radius() {
+        return radius_;
+    }
     void set_time_ptr(TimePtr t);
     void set_printer(PrintPtr printer);
 
@@ -208,8 +204,7 @@ class Entity : public std::enable_shared_from_this<Entity> {
  protected:
     ID id_;
 
-    scrimmage_proto::ContactVisualPtr visual_ =
-        std::make_shared<scrimmage_proto::ContactVisual>();
+    scrimmage_proto::ContactVisualPtr visual_ = std::make_shared<scrimmage_proto::ContactVisual>();
 
     std::vector<ControllerPtr> controllers_;
     MotionModelPtr motion_model_;
@@ -240,7 +235,7 @@ class Entity : public std::enable_shared_from_this<Entity> {
 
     double radius_ = 1;
 
-    void print(const std::string &msg);
+    void print(const std::string& msg);
     PluginManagerPtr plugin_manager_;
     FileSearchPtr file_search_;
     PubSubPtr pubsub_;
@@ -251,5 +246,5 @@ class Entity : public std::enable_shared_from_this<Entity> {
 };
 
 using EntityPtr = std::shared_ptr<Entity>;
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_ENTITY_ENTITY_H_
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_ENTITY_ENTITY_H_
diff --git a/include/scrimmage/entity/EntityPlugin.h b/include/scrimmage/entity/EntityPlugin.h
index 2946d432b8..b6b0cf9dc7 100644
--- a/include/scrimmage/entity/EntityPlugin.h
+++ b/include/scrimmage/entity/EntityPlugin.h
@@ -33,25 +33,24 @@
 #ifndef INCLUDE_SCRIMMAGE_ENTITY_ENTITYPLUGIN_H_
 #define INCLUDE_SCRIMMAGE_ENTITY_ENTITYPLUGIN_H_
 
-#include <Eigen/Dense>
-
-#include <scrimmage/plugin_manager/Plugin.h>
-#include <scrimmage/common/VariableIO.h>
 #include <scrimmage/common/ParameterServer.h>
+#include <scrimmage/common/VariableIO.h>
+#include <scrimmage/plugin_manager/Plugin.h>
 #include <scrimmage/pubsub/PubSub.h>
 #include <scrimmage/pubsub/Subscriber.h>
 
-#include <unordered_set>
-#include <unordered_map>
-#include <memory>
-#include <map>
+#include <Eigen/Dense>
 #include <list>
+#include <map>
+#include <memory>
 #include <string>
+#include <unordered_map>
+#include <unordered_set>
 
 namespace scrimmage_proto {
 class Shape;
 using ShapePtr = std::shared_ptr<Shape>;
-}
+}  // namespace scrimmage_proto
 
 namespace scrimmage {
 
@@ -72,49 +71,55 @@ class EntityPlugin : public Plugin {
     EntityPlugin();
     virtual ~EntityPlugin();
 
-    virtual bool ready() { return true; }
+    virtual bool ready() {
+        return true;
+    }
 
-    void close_plugin(const double &t);
-    virtual void close(double /*t*/) {}
+    void close_plugin(const double& t);
+    virtual void close(double /*t*/) {
+    }
 
     virtual void set_parent(EntityPtr parent);
     virtual EntityPtr parent();
 
-    void set_scoped_property(const std::string &property_name, const MessageBasePtr &property);
-    MessageBasePtr get_scoped_property_helper(const std::string &property_name);
+    void set_scoped_property(const std::string& property_name, const MessageBasePtr& property);
+    MessageBasePtr get_scoped_property_helper(const std::string& property_name);
 
     /* Homogeneous transform from parent link */
-    StatePtr transform() { return transform_; }
+    StatePtr transform() {
+        return transform_;
+    }
 
-    virtual void set_id_to_team_map(std::shared_ptr<std::unordered_map<int, int>> lookup)
-    { id_to_team_map_ = lookup; }
+    virtual void set_id_to_team_map(std::shared_ptr<std::unordered_map<int, int>> lookup) {
+        id_to_team_map_ = lookup;
+    }
 
-    virtual void set_id_to_ent_map(std::shared_ptr<std::unordered_map<int, EntityPtr>> lookup)
-    { id_to_ent_map_ = lookup; }
+    virtual void set_id_to_ent_map(std::shared_ptr<std::unordered_map<int, EntityPtr>> lookup) {
+        id_to_ent_map_ = lookup;
+    }
 
-    std::list<scrimmage_proto::ShapePtr> &shapes();
+    std::list<scrimmage_proto::ShapePtr>& shapes();
 
-    VariableIO &vars() { return vars_; }
+    VariableIO& vars() {
+        return vars_;
+    }
 
     template <class T, class CallbackFunc>
-    SubscriberBasePtr subscribe(const std::string &network_name,
-                                const std::string &topic,
+    SubscriberBasePtr subscribe(const std::string& network_name, const std::string& topic,
                                 CallbackFunc callback) {
         SubscriberBasePtr sub =
-            pubsub_->subscribe<T>(network_name, topic, callback,
-                                  0, false, std::static_pointer_cast<EntityPlugin>(shared_from_this()));
+            pubsub_->subscribe<T>(network_name, topic, callback, 0, false,
+                                  std::static_pointer_cast<EntityPlugin>(shared_from_this()));
         subs_.push_back(sub);
         return sub;
     }
 
     template <class T, class CallbackFunc>
-    SubscriberBasePtr subscribe(const std::string &network_name,
-                                const std::string &topic,
-                                CallbackFunc callback,
-                                unsigned int max_queue_size) {
-        SubscriberBasePtr sub  =
-            pubsub_->subscribe<T>(network_name, topic, callback,
-                                  max_queue_size, true, std::static_pointer_cast<EntityPlugin>(shared_from_this()));
+    SubscriberBasePtr subscribe(const std::string& network_name, const std::string& topic,
+                                CallbackFunc callback, unsigned int max_queue_size) {
+        SubscriberBasePtr sub =
+            pubsub_->subscribe<T>(network_name, topic, callback, max_queue_size, true,
+                                  std::static_pointer_cast<EntityPlugin>(shared_from_this()));
         subs_.push_back(sub);
         return sub;
     }
@@ -123,46 +128,65 @@ class EntityPlugin : public Plugin {
     PublisherPtr advertise(std::string network_name, std::string topic,
                            unsigned int max_queue_size);
 
-    void set_pubsub(PubSubPtr pubsub) { pubsub_ = pubsub; }
-    PubSubPtr pubsub() { return pubsub_; }
+    void set_pubsub(PubSubPtr pubsub) {
+        pubsub_ = pubsub;
+    }
+    PubSubPtr pubsub() {
+        return pubsub_;
+    }
 
-    std::list<SubscriberBasePtr> subs() { return subs_; }
+    std::list<SubscriberBasePtr> subs() {
+        return subs_;
+    }
 
-    void set_time(const std::shared_ptr<Time> &time) { time_ = time; }
+    void set_time(const std::shared_ptr<Time>& time) {
+        time_ = time;
+    }
     // cppcheck-suppress passedByValue
-    void set_time(std::shared_ptr<const Time> time) { time_ = time; }
-    std::shared_ptr<const Time> getTime() { return time_; }
+    void set_time(std::shared_ptr<const Time> time) {
+        time_ = time;
+    }
+    std::shared_ptr<const Time> getTime() {
+        return time_;
+    }
 
     void draw_shape(scrimmage_proto::ShapePtr s);
     bool print_err_on_exit = true;
 
-    void set_param_server(const ParameterServerPtr &param_server);
+    void set_param_server(const ParameterServerPtr& param_server);
 
     template <class T>
-    bool register_param(const std::string &name, T &variable,
-                        std::function<void(const T &value)> callback =
-                        [](const T &value){}) {
+    bool register_param(
+        const std::string& name, T& variable,
+        std::function<void(const T& value)> callback = [](const T& value) {}) {
         return param_server_->register_param<T>(
-            name, variable, callback,
-            std::static_pointer_cast<EntityPlugin>(shared_from_this()));
+            name, variable, callback, std::static_pointer_cast<EntityPlugin>(shared_from_this()));
     }
 
     template <class T>
-    bool set_param(const std::string &name, const T &value) {
+    bool set_param(const std::string& name, const T& value) {
         return param_server_->set_param<T>(name, value);
     }
 
     template <class T>
-    bool unregister_param(const std::string &name) {
+    bool unregister_param(const std::string& name) {
         return param_server_->unregister_param<T>(
             name, std::static_pointer_cast<EntityPlugin>(shared_from_this()));
     }
 
     // loop rate control
-    const double &loop_rate() const { return loop_rate_; }
-    const double &loop_timer() const { return loop_timer_; }
-    void set_loop_rate(const double &loop_rate) { loop_rate_ = loop_rate; }
-    void set_loop_timer(const double &loop_timer) { loop_timer_ = loop_timer; }
+    const double& loop_rate() const {
+        return loop_rate_;
+    }
+    const double& loop_timer() const {
+        return loop_timer_;
+    }
+    void set_loop_rate(const double& loop_rate) {
+        loop_rate_ = loop_rate;
+    }
+    void set_loop_timer(const double& loop_timer) {
+        loop_timer_ = loop_timer;
+    }
     bool step_loop_timer(double dt);
 
  protected:
@@ -191,5 +215,5 @@ class EntityPlugin : public Plugin {
     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };
 using EntityPluginPtr = std::shared_ptr<EntityPlugin>;
-} // namespace scrimmage
+}  // namespace scrimmage
 #endif  // INCLUDE_SCRIMMAGE_ENTITY_ENTITYPLUGIN_H_
diff --git a/include/scrimmage/entity/EntityPluginHelper.h b/include/scrimmage/entity/EntityPluginHelper.h
index 42109eebfd..61075092fe 100644
--- a/include/scrimmage/entity/EntityPluginHelper.h
+++ b/include/scrimmage/entity/EntityPluginHelper.h
@@ -33,50 +33,39 @@
 #ifndef INCLUDE_SCRIMMAGE_ENTITY_ENTITYPLUGINHELPER_H_
 #define INCLUDE_SCRIMMAGE_ENTITY_ENTITYPLUGINHELPER_H_
 
-#include <scrimmage/fwd_decl.h>
 #include <scrimmage/autonomy/Autonomy.h>
-#include <scrimmage/parse/ConfigParse.h>
+#include <scrimmage/fwd_decl.h>
 #include <scrimmage/motion/Controller.h>
+#include <scrimmage/parse/ConfigParse.h>
 #include <scrimmage/plugin_manager/PluginManager.h>
 
+#include <boost/optional.hpp>
+#include <functional>
 #include <map>
-#include <unordered_map>
+#include <memory>
 #include <set>
-#include <vector>
 #include <string>
-#include <functional>
-#include <memory>
-
-#include <boost/optional.hpp>
+#include <unordered_map>
+#include <vector>
 
 namespace scrimmage {
 
 template <class T>
 boost::optional<std::shared_ptr<T>> make_autonomy(
-    const std::string& autonomy_name,
-    PluginManagerPtr plugin_manager,
-    std::map<std::string, std::string> &overrides,
-    EntityPtr parent,
-    StatePtr state,
-    std::shared_ptr<std::unordered_map<int, int>> &id_to_team_map,
-    std::shared_ptr<std::unordered_map<int, EntityPtr>> &id_to_ent_map,
-    std::shared_ptr<GeographicLib::LocalCartesian> proj,
-    ContactMapPtr &contacts,
-    FileSearchPtr &file_search,
-    RTreePtr &rtree,
-    PubSubPtr &pubsub,
-    const std::shared_ptr<const Time> &time,
-    const ParameterServerPtr &param_server,
-    const std::set<std::string> &plugin_tags = {},
-    std::function<void(std::map<std::string, std::string>&)> param_override_func = [](std::map<std::string, std::string>& params){},
-    std::vector<ControllerPtr> controllers = {},
-    const int& debug_level = 0) {
-
+    const std::string& autonomy_name, PluginManagerPtr plugin_manager,
+    std::map<std::string, std::string>& overrides, EntityPtr parent, StatePtr state,
+    std::shared_ptr<std::unordered_map<int, int>>& id_to_team_map,
+    std::shared_ptr<std::unordered_map<int, EntityPtr>>& id_to_ent_map,
+    std::shared_ptr<GeographicLib::LocalCartesian> proj, ContactMapPtr& contacts,
+    FileSearchPtr& file_search, RTreePtr& rtree, PubSubPtr& pubsub,
+    const std::shared_ptr<const Time>& time, const ParameterServerPtr& param_server,
+    const std::set<std::string>& plugin_tags = {},
+    std::function<void(std::map<std::string, std::string>&)> param_override_func =
+        [](std::map<std::string, std::string>& params) {},
+    std::vector<ControllerPtr> controllers = {}, const int& debug_level = 0) {
     ConfigParse config_parse;
-    PluginStatus<T> status =
-            plugin_manager->make_plugin<T>(
-                "scrimmage::Autonomy", autonomy_name, *file_search,
-                config_parse, overrides, plugin_tags);
+    PluginStatus<T> status = plugin_manager->make_plugin<T>(
+        "scrimmage::Autonomy", autonomy_name, *file_search, config_parse, overrides, plugin_tags);
     if (status.status == PluginStatus<T>::cast_failed) {
         std::cout << "Failed to open autonomy plugin: " << autonomy_name << std::endl;
     } else if (status.status == PluginStatus<T>::parse_failed) {
@@ -118,5 +107,5 @@ boost::optional<std::shared_ptr<T>> make_autonomy(
     }
     return boost::none;
 }
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_ENTITY_ENTITYPLUGINHELPER_H_
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_ENTITY_ENTITYPLUGINHELPER_H_
diff --git a/include/scrimmage/entity/External.h b/include/scrimmage/entity/External.h
index c5cb9c4430..e665e266fe 100644
--- a/include/scrimmage/entity/External.h
+++ b/include/scrimmage/entity/External.h
@@ -33,9 +33,9 @@
 #define INCLUDE_SCRIMMAGE_ENTITY_EXTERNAL_H_
 
 #include <scrimmage/autonomy/Autonomy.h>
+#include <scrimmage/common/DelayedTask.h>
 #include <scrimmage/common/RTree.h>
 #include <scrimmage/common/Time.h>
-#include <scrimmage/common/DelayedTask.h>
 #include <scrimmage/entity/Entity.h>
 #include <scrimmage/log/Log.h>
 #include <scrimmage/log/Print.h>
@@ -43,25 +43,23 @@
 #include <scrimmage/motion/Controller.h>
 #include <scrimmage/proto/ProtoConversions.h>
 #include <scrimmage/proto/Shape.pb.h>
-#include <scrimmage/pubsub/Subscriber.h>
 #include <scrimmage/pubsub/Publisher.h>
+#include <scrimmage/pubsub/Subscriber.h>
 
 #include <algorithm>
+#include <boost/bind.hpp>
+#include <boost/function.hpp>
+#include <boost/type_index.hpp>
 #include <functional>
+#include <iostream>
 #include <list>
 #include <map>
+#include <memory>
+#include <mutex>  // NOLINT
+#include <queue>
 #include <set>
 #include <string>
-#include <mutex> // NOLINT
-#include <queue>
 #include <unordered_map>
-#include <memory>
-
-#include <iostream>
-
-#include <boost/type_index.hpp>
-#include <boost/bind.hpp>
-#include <boost/function.hpp>
 
 namespace scrimmage {
 
@@ -76,35 +74,35 @@ using GlobalServicePtr = std::shared_ptr<GlobalService>;
 // c++17 version as the cutoff since c++20 features begin appearing after that
 // but before a finalized c++20 version number has been designated. E.g. gcc9,
 // if compiling with c++20, has a __cplusplus value of 201709L.
-#if __cplusplus <= 201703L // pre-c++20
-    #define __copy_capture_with_this =
-#else // c++20 and beyond
-    #define __copy_capture_with_this =, this
+#if __cplusplus <= 201703L  // pre-c++20
+#define __copy_capture_with_this =
+#else  // c++20 and beyond
+#define __copy_capture_with_this =, this
 #endif
 
 class External {
  public:
     External();
-    EntityPtr &entity();
-    bool create_entity(const std::string &mission_file,
-                       const std::string &entity_tag,
-                       const std::string &plugin_tags_str,
-                       int entity_id,
-                       int max_entities,
-                       double init_time,
-                       double init_dt,
-                       const std::string &log_dir,
-                       std::function<void(std::map<std::string, std::string>&)> param_override_func = [](std::map<std::string, std::string>&){},
-                       const std::string& mission_file_overrides = "",
-                       const int& debug_level = 0);
+    EntityPtr& entity();
+    bool create_entity(
+        const std::string& mission_file, const std::string& entity_tag,
+        const std::string& plugin_tags_str, int entity_id, int max_entities, double init_time,
+        double init_dt, const std::string& log_dir,
+        std::function<void(std::map<std::string, std::string>&)> param_override_func =
+            [](std::map<std::string, std::string>&) {},
+        const std::string& mission_file_overrides = "", const int& debug_level = 0);
     void close();
     bool create_interactions();
 
     double motion_dt_ = 1;
 
-    std::size_t& shape_queue_max_size() { return shape_queue_max_size_; }
+    std::size_t& shape_queue_max_size() {
+        return shape_queue_max_size_;
+    }
     using ShapeQueue = std::queue<scrimmage_proto::ShapePtr>;
-    ShapeQueue& shape_queue() { return shape_queue_; }
+    ShapeQueue& shape_queue() {
+        return shape_queue_;
+    }
 
     VariableIO vars;
     std::mutex mutex;
@@ -112,17 +110,19 @@ class External {
     MissionParsePtr mp();
     bool send_messages();
 
-    void print_plugins(std::ostream &out) const;
+    void print_plugins(std::ostream& out) const;
 
-    ParameterServerPtr param_server() { return param_server_; }
+    ParameterServerPtr param_server() {
+        return param_server_;
+    }
 
-    bool enable_outgoing_interface(const std::string &ip, const int &port);
+    bool enable_outgoing_interface(const std::string& ip, const int& port);
     bool disable_outgoing_interfaces();
 
-    void set_time(const double &t, const double &dt);
+    void set_time(const double& t, const double& dt);
 
  protected:
-    void setup_logging(const std::string &log_dir);
+    void setup_logging(const std::string& log_dir);
     void update_ents();
     EntityPtr entity_;
     bool enable_motion_ = false;
@@ -156,9 +156,8 @@ class External {
 
  public:
     template <class ScType, class Sc2Ros, class CallbackFunc>
-    void pub_cb(std::string network_name, std::string topic_name,
-                Sc2Ros sc2ros, CallbackFunc func) {
-
+    void pub_cb(std::string network_name, std::string topic_name, Sc2Ros sc2ros,
+                CallbackFunc func) {
         boost::optional<std::list<NetworkDevicePtr>> pubs =
             pubsub_->find_pubs(network_name, topic_name);
 
@@ -183,24 +182,21 @@ class External {
     }
 
     template <class ScType, class Sc2Ros>
-    void pub_cb(std::string network_name, std::string topic_name,
-                Sc2Ros sc2ros, ros::Publisher ros_pub) {
-
+    void pub_cb(std::string network_name, std::string topic_name, Sc2Ros sc2ros,
+                ros::Publisher ros_pub) {
         auto ros_pub_ptr = std::make_shared<ros::Publisher>(ros_pub);
-        auto callback = [=](auto foo) {
-            ros_pub_ptr->publish(foo);
-        };
+        auto callback = [=](auto foo) { ros_pub_ptr->publish(foo); };
         pub_cb<ScType, Sc2Ros>(network_name, topic_name, sc2ros, callback);
     }
 
     template <class RosType, class Ros2Sc>
-    boost::function<void(const boost::shared_ptr<RosType const>&)>
-    sub_cb(std::string network_name, std::string topic_name, Ros2Sc ros2sc) {
-
+    boost::function<void(const boost::shared_ptr<RosType const>&)> sub_cb(std::string network_name,
+                                                                          std::string topic_name,
+                                                                          Ros2Sc ros2sc) {
         boost::optional<std::list<NetworkDevicePtr>> subs =
             pubsub_->find_subs(network_name, topic_name);
 
-        return [__copy_capture_with_this](const boost::shared_ptr<RosType const> &ros_msg) {
+        return [__copy_capture_with_this](const boost::shared_ptr<RosType const>& ros_msg) {
             boost::optional<std::list<NetworkDevicePtr>> curr_subs =
                 pubsub_->find_subs(network_name, topic_name);
             if (!curr_subs) {
@@ -211,7 +207,6 @@ class External {
             call_update_contacts(time_->t());
             mutex.lock();
 
-
             auto sc_msg = std::make_shared<Message<ScType>>(ros2sc(*ros_msg));
 
             for (auto sub : *curr_subs) {
@@ -219,7 +214,8 @@ class External {
                 if (sub_cast) {
                     sub_cast->accept(sc_msg);
                 } else {
-                    std::cout << "Error: could not cast to Subscriber base in callback" << std::endl;
+                    std::cout << "Error: could not cast to Subscriber base in callback"
+                              << std::endl;
                 }
             }
             send_messages();
@@ -227,29 +223,25 @@ class External {
         };
     }
 
-    template <class RosType, class ScrimmageResponseType,
-              class Sc2RosResFunc>
-    boost::function<bool(typename RosType::Request &, typename RosType::Response &)>
-    srv_cb(const std::string &service_name,
-           scrimmage::Service sc_service_func,
-           Sc2RosResFunc sc2ros_response_func) {
-        auto ros2sc_req_func = [&](typename RosType::Request &/*ros_req*/) {return 0;};
-        return srv_cb<RosType, ScrimmageResponseType>(
-            service_name, sc_service_func, ros2sc_req_func, sc2ros_response_func);
+    template <class RosType, class ScrimmageResponseType, class Sc2RosResFunc>
+    boost::function<bool(typename RosType::Request&, typename RosType::Response&)> srv_cb(
+        const std::string& service_name, scrimmage::Service sc_service_func,
+        Sc2RosResFunc sc2ros_response_func) {
+        auto ros2sc_req_func = [&](typename RosType::Request& /*ros_req*/) { return 0; };
+        return srv_cb<RosType, ScrimmageResponseType>(service_name, sc_service_func,
+                                                      ros2sc_req_func, sc2ros_response_func);
     }
 
-    template <class RosType, class ScrimmageResponseType,
-              class Ros2ScRequestFunc, class Sc2RosResFunc>
-    boost::function<bool(typename RosType::Request &, typename RosType::Response &)>
-    srv_cb(const std::string &service_name,
-           scrimmage::Service sc_service_func,
-           Ros2ScRequestFunc ros2sc_request_func,
-           Sc2RosResFunc sc2ros_response_func) {
-
-        return [__copy_capture_with_this](typename RosType::Request &ros_req, typename RosType::Response &ros_res) {
-            auto err_msg = [&](const std::string &preface) {
-                std::cout << preface << " in advertised_service \""
-                    << service_name << "\"" << std::endl;
+    template <class RosType, class ScrimmageResponseType, class Ros2ScRequestFunc,
+              class Sc2RosResFunc>
+    boost::function<bool(typename RosType::Request&, typename RosType::Response&)> srv_cb(
+        const std::string& service_name, scrimmage::Service sc_service_func,
+        Ros2ScRequestFunc ros2sc_request_func, Sc2RosResFunc sc2ros_response_func) {
+        return [__copy_capture_with_this](typename RosType::Request& ros_req,
+                                          typename RosType::Response& ros_res) {
+            auto err_msg = [&](const std::string& preface) {
+                std::cout << preface << " in advertised_service \"" << service_name << "\""
+                          << std::endl;
             };
 
             call_update_contacts(time_->t());
@@ -267,9 +259,8 @@ class External {
             if (!sc_res) {
                 std::stringstream ss;
                 ss << "could not cast to scrimmage::MessagePtr<ScrimmageResponseType> "
-                    << "(aka, scrimmage::MessagePtr<"
-                    << boost::typeindex::type_id<ScrimmageResponseType>().pretty_name()
-                    << ">)";
+                   << "(aka, scrimmage::MessagePtr<"
+                   << boost::typeindex::type_id<ScrimmageResponseType>().pretty_name() << ">)";
                 err_msg(ss.str());
                 return false;
             }
@@ -280,74 +271,63 @@ class External {
     }
 
     template <class RosType, class Ros2ScResFunc>
-    void add_srv_client(ros::NodeHandle &nh,
-        const std::string &sc_topic,
-        Ros2ScResFunc ros2sc_response_func,
-        const std::string &ros_topic = "") {
-
-        auto nop = [&](auto msg){};
-        add_srv_client<RosType, void>(
-            nh, sc_topic, nop, ros2sc_response_func, ros_topic);
+    void add_srv_client(ros::NodeHandle& nh, const std::string& sc_topic,
+                        Ros2ScResFunc ros2sc_response_func, const std::string& ros_topic = "") {
+        auto nop = [&](auto msg) {};
+        add_srv_client<RosType, void>(nh, sc_topic, nop, ros2sc_response_func, ros_topic);
     }
 
     template <class RosType, class ScReqType, class Sc2RosReqFunc, class Ros2ScResFunc>
-    void add_srv_client(ros::NodeHandle &nh,
-        const std::string &sc_topic,
-        Sc2RosReqFunc sc2ros_request_func,
-        Ros2ScResFunc ros2sc_response_func,
-        const std::string &ros_topic = "") {
-
+    void add_srv_client(ros::NodeHandle& nh, const std::string& sc_topic,
+                        Sc2RosReqFunc sc2ros_request_func, Ros2ScResFunc ros2sc_response_func,
+                        const std::string& ros_topic = "") {
         std::string topic = ros_topic == "" ? sc_topic : ros_topic;
         auto service_client =
             std::make_shared<ros::ServiceClient>(nh.serviceClient<RosType>(topic));
 
-        auto call_service =
-            [__copy_capture_with_this](scrimmage::MessageBasePtr sc_req, scrimmage::MessageBasePtr &sc_res) {
-                std::string suffix =
-                    std::string("(sc_topic = ") + sc_topic + ", ros_topic = " + topic + ")";
+        auto call_service = [__copy_capture_with_this](scrimmage::MessageBasePtr sc_req,
+                                                       scrimmage::MessageBasePtr& sc_res) {
+            std::string suffix =
+                std::string("(sc_topic = ") + sc_topic + ", ros_topic = " + topic + ")";
 
-                RosType srv;
-                create_ros_req<ScReqType, Sc2RosReqFunc, RosType>(
-                    sc_req, sc2ros_request_func, suffix, srv);
+            RosType srv;
+            create_ros_req<ScReqType, Sc2RosReqFunc, RosType>(sc_req, sc2ros_request_func, suffix,
+                                                              srv);
 
-                if (!service_client->call(srv)) {
-                    std::cout << "service call failed " << suffix << std::endl;
-                    return false;
-                }
+            if (!service_client->call(srv)) {
+                std::cout << "service call failed " << suffix << std::endl;
+                return false;
+            }
 
-                using ScResType = decltype(ros2sc_response_func(srv.response));
-                auto sc_res_cast = std::make_shared<Message<ScResType>>(ros2sc_response_func(srv.response));
-                sc_res = std::dynamic_pointer_cast<MessageBase>(sc_res_cast);
-                return true;
-            };
+            using ScResType = decltype(ros2sc_response_func(srv.response));
+            auto sc_res_cast =
+                std::make_shared<Message<ScResType>>(ros2sc_response_func(srv.response));
+            sc_res = std::dynamic_pointer_cast<MessageBase>(sc_res_cast);
+            return true;
+        };
 
         entity_->services()[sc_topic] = call_service;
     }
 
  protected:
-    template <class ScReqType,
-              class Sc2RosReqFunc,
-              class RosType>
+    template <class ScReqType, class Sc2RosReqFunc, class RosType>
     void
     // cppcheck-suppress passedByValue
-    create_ros_req(typename std::enable_if_t<std::is_void<ScReqType>::value, MessageBasePtr> /*sc_req*/,
-                   Sc2RosReqFunc /*sc2ros_req_func*/,
-                   const std::string &/*err_msg*/, RosType &/*srv*/) {}
+    create_ros_req(
+        typename std::enable_if_t<std::is_void<ScReqType>::value, MessageBasePtr> /*sc_req*/,
+        Sc2RosReqFunc /*sc2ros_req_func*/, const std::string& /*err_msg*/, RosType& /*srv*/) {
+    }
 
-    template <class ScReqType,
-              class Sc2RosReqFunc,
-              class RosType>
-    void
-    create_ros_req(typename std::enable_if_t<!std::is_void<ScReqType>::value, MessageBasePtr> sc_req,
-                   Sc2RosReqFunc sc2ros_req_func,
-                   const std::string &err_msg, RosType &srv) {
-        auto sc_req_cast =
-            std::dynamic_pointer_cast<Message<ScReqType>>(sc_req);
+    template <class ScReqType, class Sc2RosReqFunc, class RosType>
+    void create_ros_req(
+        typename std::enable_if_t<!std::is_void<ScReqType>::value, MessageBasePtr> sc_req,
+        Sc2RosReqFunc sc2ros_req_func, const std::string& err_msg, RosType& srv) {
+        auto sc_req_cast = std::dynamic_pointer_cast<Message<ScReqType>>(sc_req);
         if (!sc_req_cast) {
             std::cout << "could not cast scrimmage::MessageBase request "
-                << " to scrimmage::MessagePtr<"
-                << boost::typeindex::type_id<ScReqType>().pretty_name()
-                << "> " << err_msg << std::endl;
+                      << " to scrimmage::MessagePtr<"
+                      << boost::typeindex::type_id<ScReqType>().pretty_name() << "> " << err_msg
+                      << std::endl;
         }
         srv.request = sc2ros_req_func(sc_req_cast->data);
     }
@@ -355,14 +335,14 @@ class External {
 #endif
 
  protected:
-    bool call_update_contacts(double t); // false on error
+    bool call_update_contacts(double t);  // false on error
     void update_time(double t);
 
     std::list<InterfacePtr> outgoing_interfaces_;
 };
 
-} // namespace scrimmage
+}  // namespace scrimmage
 
 #undef __copy_capture_with_this
 
-#endif // INCLUDE_SCRIMMAGE_ENTITY_EXTERNAL_H_
+#endif  // INCLUDE_SCRIMMAGE_ENTITY_EXTERNAL_H_
diff --git a/include/scrimmage/fwd_decl.h b/include/scrimmage/fwd_decl.h
index 1f73f4e065..4f63abb87e 100644
--- a/include/scrimmage/fwd_decl.h
+++ b/include/scrimmage/fwd_decl.h
@@ -33,20 +33,20 @@
 #ifndef INCLUDE_SCRIMMAGE_FWD_DECL_H_
 #define INCLUDE_SCRIMMAGE_FWD_DECL_H_
 
-#include <string>
-#include <memory>
 #include <map>
+#include <memory>
+#include <string>
 #include <unordered_map>
 
 namespace GeographicLib {
 class LocalCartesian;
 using LocalCartesianPtr = std::shared_ptr<LocalCartesian>;
-}
+}  // namespace GeographicLib
 
 namespace scrimmage_proto {
 class Shape;
 using ShapePtr = std::shared_ptr<scrimmage_proto::Shape>;
-}
+}  // namespace scrimmage_proto
 
 namespace scrimmage {
 
@@ -157,6 +157,6 @@ using MetricsPtr = std::shared_ptr<Metrics>;
 using AttributeMap = std::map<std::string, std::map<std::string, std::string>>;
 
 class CameraInterface;
-} // namespace scrimmage
+}  // namespace scrimmage
 
-#endif // INCLUDE_SCRIMMAGE_FWD_DECL_H_
+#endif  // INCLUDE_SCRIMMAGE_FWD_DECL_H_
diff --git a/include/scrimmage/log/Frame.h b/include/scrimmage/log/Frame.h
index 3126205aa8..3e25a2d401 100644
--- a/include/scrimmage/log/Frame.h
+++ b/include/scrimmage/log/Frame.h
@@ -44,5 +44,5 @@ class Frame {
     double time_ = 0;
     ContactMapPtr contacts_;
 };
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_LOG_FRAME_H_
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_LOG_FRAME_H_
diff --git a/include/scrimmage/log/FrameUpdateClient.h b/include/scrimmage/log/FrameUpdateClient.h
index d1c02c40b7..d6436d78e1 100644
--- a/include/scrimmage/log/FrameUpdateClient.h
+++ b/include/scrimmage/log/FrameUpdateClient.h
@@ -33,9 +33,9 @@
 #ifndef INCLUDE_SCRIMMAGE_LOG_FRAMEUPDATECLIENT_H_
 #define INCLUDE_SCRIMMAGE_LOG_FRAMEUPDATECLIENT_H_
 
-#include <scrimmage/proto/Frame.pb.h>
 #include <scrimmage/fwd_decl.h>
 #include <scrimmage/math/Angles.h>
+#include <scrimmage/proto/Frame.pb.h>
 
 #if ENABLE_GRPC == 1
 #include <scrimmage/proto/Scrimmage.grpc.pb.h>
@@ -47,10 +47,11 @@
 namespace scrimmage {
 class FrameUpdateClient {
  public:
-    FrameUpdateClient(const std::string &ip, int port);
-    bool send_frame(scrimmage_proto::Frame &frame);
+    FrameUpdateClient(const std::string& ip, int port);
+    bool send_frame(scrimmage_proto::Frame& frame);
+
+    void set_projection(const std::shared_ptr<GeographicLib::LocalCartesian>& proj);
 
-    void set_projection(const std::shared_ptr<GeographicLib::LocalCartesian> &proj);
  private:
 #if ENABLE_GRPC == 1
     std::unique_ptr<scrimmage_proto::ScrimmageService::Stub> stub_;
@@ -58,5 +59,5 @@ class FrameUpdateClient {
     std::shared_ptr<GeographicLib::LocalCartesian> proj_;
     Angles angles_to_gps_;
 };
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_LOG_FRAMEUPDATECLIENT_H_
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_LOG_FRAMEUPDATECLIENT_H_
diff --git a/include/scrimmage/log/Log.h b/include/scrimmage/log/Log.h
index 1c6bc4e51d..2ae35a4c6e 100644
--- a/include/scrimmage/log/Log.h
+++ b/include/scrimmage/log/Log.h
@@ -33,31 +33,33 @@
 #ifndef INCLUDE_SCRIMMAGE_LOG_LOG_H_
 #define INCLUDE_SCRIMMAGE_LOG_LOG_H_
 
+#include <google/protobuf/io/zero_copy_stream_impl.h>
 #include <scrimmage/log/Frame.h>
 
-#include <list>
 #include <fstream>
 #include <iostream>
+#include <list>
 #include <map>
-#include <string>
 #include <memory>
-#include <google/protobuf/io/zero_copy_stream_impl.h>
-
+#include <string>
 
-namespace google { namespace protobuf {
+namespace google {
+namespace protobuf {
 class MessageLite;
 namespace io {
 class ZeroCopyInputStream;
 class ZeroCopyOutputStream;
 class FileOutputStreamPtr;
-}}}
+}  // namespace io
+}  // namespace protobuf
+}  // namespace google
 
 namespace scrimmage_proto {
 class Frame;
 class Shapes;
 class UTMTerrain;
 class ContactVisual;
-}
+}  // namespace scrimmage_proto
 
 namespace scrimmage {
 
@@ -79,49 +81,41 @@ class Log {
         MSG = 6
     };
 
-    enum Mode {
-        READ = 0,
-        WRITE = 1,
-        NONE
-    };
+    enum Mode { READ = 0, WRITE = 1, NONE };
 
-    bool init(const std::string &dir, Mode mode);
+    bool init(const std::string& dir, Mode mode);
 
     bool parse(std::string dir);
     bool parse(std::string filename, FileType type);
 
-    bool parse_frames(std::string filename,
-                      ZeroCopyInputStreamPtr input);
+    bool parse_frames(std::string filename, ZeroCopyInputStreamPtr input);
 
-    bool parse_shapes(std::string filename,
-                      ZeroCopyInputStreamPtr input);
+    bool parse_shapes(std::string filename, ZeroCopyInputStreamPtr input);
 
-    bool parse_utm_terrain(std::string filename,
-                           ZeroCopyInputStreamPtr input);
+    bool parse_utm_terrain(std::string filename, ZeroCopyInputStreamPtr input);
 
-    bool parse_contact_visual(std::string filename,
-                              ZeroCopyInputStreamPtr input);
+    bool parse_contact_visual(std::string filename, ZeroCopyInputStreamPtr input);
 
     bool close_log();
 
-    std::list<Frame> & scrimmage_frames();
+    std::list<Frame>& scrimmage_frames();
 
-    std::list<std::shared_ptr<scrimmage_proto::Frame>> & frames();
-    std::list<std::shared_ptr<scrimmage_proto::Shapes>> & shapes();
-    std::list<std::shared_ptr<scrimmage_proto::UTMTerrain>> & utm_terrain();
-    std::list<std::shared_ptr<scrimmage_proto::ContactVisual>> & contact_visual();
+    std::list<std::shared_ptr<scrimmage_proto::Frame>>& frames();
+    std::list<std::shared_ptr<scrimmage_proto::Shapes>>& shapes();
+    std::list<std::shared_ptr<scrimmage_proto::UTMTerrain>>& utm_terrain();
+    std::list<std::shared_ptr<scrimmage_proto::ContactVisual>>& contact_visual();
 
     std::string log_dir();
 
-    bool write_ascii(const std::string &str);
+    bool write_ascii(const std::string& str);
 
-    bool save_frame(const std::shared_ptr<scrimmage_proto::Frame> &frame);
+    bool save_frame(const std::shared_ptr<scrimmage_proto::Frame>& frame);
 
-    bool save_shapes(const scrimmage_proto::Shapes &shapes);
+    bool save_shapes(const scrimmage_proto::Shapes& shapes);
 
-    bool save_utm_terrain(const std::shared_ptr<scrimmage_proto::UTMTerrain> &utm_terrain);
+    bool save_utm_terrain(const std::shared_ptr<scrimmage_proto::UTMTerrain>& utm_terrain);
 
-    bool save_contact_visual(const std::shared_ptr<scrimmage_proto::ContactVisual> &contact_visual);
+    bool save_contact_visual(const std::shared_ptr<scrimmage_proto::ContactVisual>& contact_visual);
 
     // bool save_messages();
 
@@ -143,7 +137,7 @@ class Log {
     bool drop_bin_logging_ = false;
     Mode mode_ = Mode::READ;
 
-    bool open_file(std::string name, int &fd);
+    bool open_file(std::string name, int& fd);
 
     std::string frames_name_ = "frames.bin";
     std::string shapes_name_ = "shapes.bin";
@@ -169,29 +163,21 @@ class Log {
     ZeroCopyOutputStreamPtr msgs_output_;
 
     std::list<Frame> scrimmage_frames_;
-    std::list<std::shared_ptr<scrimmage_proto::Frame> > frames_;
-    std::list<std::shared_ptr<scrimmage_proto::Shapes> > shapes_;
-    std::list<std::shared_ptr<scrimmage_proto::UTMTerrain> > utm_terrain_;
-    std::list<std::shared_ptr<scrimmage_proto::ContactVisual> > contact_visual_;
+    std::list<std::shared_ptr<scrimmage_proto::Frame>> frames_;
+    std::list<std::shared_ptr<scrimmage_proto::Shapes>> shapes_;
+    std::list<std::shared_ptr<scrimmage_proto::UTMTerrain>> utm_terrain_;
+    std::list<std::shared_ptr<scrimmage_proto::ContactVisual>> contact_visual_;
 
-    bool writeDelimitedTo(
-        const google::protobuf::MessageLite& message,
-        ZeroCopyOutputStreamPtr rawOutput);
+    bool writeDelimitedTo(const google::protobuf::MessageLite& message,
+                          ZeroCopyOutputStreamPtr rawOutput);
 
-    bool readDelimitedFrom(const std::string &filename,
-                           ZeroCopyInputStreamPtr rawInput,
-                           MessageLitePtr message,
-                           bool& clean_eof);
+    bool readDelimitedFrom(const std::string& filename, ZeroCopyInputStreamPtr rawInput,
+                           MessageLitePtr message, bool& clean_eof);
 
     bool close_fileoutputstream(ZeroCopyOutputStreamPtr stream);
 
     template <class T, class T2>
-    bool parse_proto(
-        T factory,
-        T2 &container,
-        const std::string &path,
-        const std::string &prefix
-    ) {
+    bool parse_proto(T factory, T2& container, const std::string& path, const std::string& prefix) {
         int ct = 0;
         while (true) {
             auto temp_path = path + "/" + prefix + "_" + std::to_string(ct) + ".bin";
@@ -210,7 +196,6 @@ class Log {
         }
         return true;
     }
-
 };
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_LOG_LOG_H_
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_LOG_LOG_H_
diff --git a/include/scrimmage/log/Print.h b/include/scrimmage/log/Print.h
index 9f981d6bf3..c3a8f89253 100644
--- a/include/scrimmage/log/Print.h
+++ b/include/scrimmage/log/Print.h
@@ -34,88 +34,72 @@
 #include <scrimmage/fwd_decl.h>
 #include <scrimmage/plugin_manager/Plugin.h>
 
-#include <memory>
+#include <cmath>
 #include <fstream>
+#include <memory>
 #include <ostream>
-#include <cmath>
 
 namespace scrimmage {
 
 class Time;
 using TimePtr = std::shared_ptr<Time>;
 
-
-struct PrintEnums
-{
-    enum class WARN_LEVEL : char {
-        DEV = 0,
-        INFO = 1,
-        WARNING = 2,
-        ERROR = 3
-    };
-    enum class WRITE_TO : char {
-        BOTH = 0,
-        FILE_ONLY = 1,
-        CONSOLE_ONLY = 2
-    };
+struct PrintEnums {
+    enum class WARN_LEVEL : char { DEV = 0, INFO = 1, WARNING = 2, ERROR = 3 };
+    enum class WRITE_TO : char { BOTH = 0, FILE_ONLY = 1, CONSOLE_ONLY = 2 };
 };
 
-
-
-
-
 struct PrintData {
-  PrintData() : time_(std::nan("")), name_("UnknownClass"), entity_id_(-1) {}
-  double time_;
-  std::string name_;
-  int entity_id_;
+    PrintData() : time_(std::nan("")), name_("UnknownClass"), entity_id_(-1) {
+    }
+    double time_;
+    std::string name_;
+    int entity_id_;
 };
 
-
-
-
-
 class Print {
-  public:
-    Print() : time_(nullptr), log_dir_(""), output_flag_(PrintEnums::WRITE_TO::BOTH) {}
+ public:
+    Print() : time_(nullptr), log_dir_(""), output_flag_(PrintEnums::WRITE_TO::BOTH) {
+    }
 
-    void init(TimePtr &time, std::string log_dir);
-    void init(TimePtr &time, std::string log_dir, PrintEnums::WRITE_TO flag);
+    void init(TimePtr& time, std::string log_dir);
+    void init(TimePtr& time, std::string log_dir, PrintEnums::WRITE_TO flag);
     void close();
     void flush();
 
-    void printDev(PrintData &data, std::string outmessage);
-    void printInfo(PrintData &data, std::string outmessage);
-    void printWarning(PrintData &data, std::string outmessage);
-    void printError(PrintData &data, std::string outmessage);
+    void printDev(PrintData& data, std::string outmessage);
+    void printInfo(PrintData& data, std::string outmessage);
+    void printWarning(PrintData& data, std::string outmessage);
+    void printError(PrintData& data, std::string outmessage);
 
-    void printDev(EntityPlugin &caller, std::string outmessage);
-    void printInfo(EntityPlugin &caller, std::string outmessage);
-    void printWarning(EntityPlugin &caller, std::string outmessage);
-    void printError(EntityPlugin &caller, std::string outmessage);
+    void printDev(EntityPlugin& caller, std::string outmessage);
+    void printInfo(EntityPlugin& caller, std::string outmessage);
+    void printWarning(EntityPlugin& caller, std::string outmessage);
+    void printError(EntityPlugin& caller, std::string outmessage);
 
-  protected:
+ protected:
     static const char* getWarningLevel(PrintEnums::WARN_LEVEL e);
 
     static std::string formatTime(double time);
-    static std::string formatMsg(PrintEnums::WARN_LEVEL level, PrintData &data, std::string msg);
+    static std::string formatMsg(PrintEnums::WARN_LEVEL level, PrintData& data, std::string msg);
 
-    void print(std::ostream& stream, PrintEnums::WARN_LEVEL level, EntityPlugin &caller, std::string msg);
-    void print(std::ostream& stream, PrintEnums::WARN_LEVEL level, PrintData &data, std::string msg);
+    void print(std::ostream& stream, PrintEnums::WARN_LEVEL level, EntityPlugin& caller,
+               std::string msg);
+    void print(std::ostream& stream, PrintEnums::WARN_LEVEL level, PrintData& data,
+               std::string msg);
 
-  private:
+ private:
     TimePtr time_;
     std::string log_dir_;
     std::ofstream log_stream_;
     PrintEnums::WRITE_TO output_flag_;
-  
+
     bool write_file();
     bool write_console();
-
-  };
+};
 
 using PrintPtr = std::shared_ptr<Print>;
 
-} // namespace scrimmage
+}  // namespace scrimmage
 
-#endif // INCLUDE_SCRIMMAGE_LOG_PRINT_H_
+#endif  // INCLUDE_SCRIMMAGE_LOG_PRINT_H_
diff --git a/include/scrimmage/math/Angles.h b/include/scrimmage/math/Angles.h
index 29ed4f9d48..596299e340 100644
--- a/include/scrimmage/math/Angles.h
+++ b/include/scrimmage/math/Angles.h
@@ -38,28 +38,20 @@ class Angles {
  public:
     Angles();
 
-    enum class Type {GPS, EUCLIDEAN};
+    enum class Type { GPS, EUCLIDEAN };
     Angles(double angle, Type input_type, Type output_type);
 
-    enum class Rotate {
-        CCW = 0,
-        CW
-    };
-
-///           Pos_Y
-///             |
-///             |
-///   Neg_X --- 0 --- Pos_X
-///             |
-///             |
-///           Neg_Y
-
-    enum class HeadingZero {
-        Pos_X = 1,
-        Pos_Y = 2,
-        Neg_X = 3,
-        Neg_Y = 4
-    };
+    enum class Rotate { CCW = 0, CW };
+
+    ///           Pos_Y
+    ///             |
+    ///             |
+    ///   Neg_X --- 0 --- Pos_X
+    ///             |
+    ///             |
+    ///           Neg_Y
+
+    enum class HeadingZero { Pos_X = 1, Pos_Y = 2, Neg_X = 3, Neg_Y = 4 };
 
     // Degrees to/from radians conversion
     static double deg2rad(double deg);
@@ -125,5 +117,5 @@ class Angles {
 
  private:
 };
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_MATH_ANGLES_H_
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_MATH_ANGLES_H_
diff --git a/include/scrimmage/math/Quaternion.h b/include/scrimmage/math/Quaternion.h
index bddeb5b413..4d11d6132f 100644
--- a/include/scrimmage/math/Quaternion.h
+++ b/include/scrimmage/math/Quaternion.h
@@ -47,19 +47,17 @@ class Quaternion : public Eigen::Quaternion<double, Eigen::DontAlign> {
      */
     Quaternion();
 
-    Quaternion(const Quaternion &other);
-    explicit Quaternion(const Eigen::Quaternion<double, Eigen::DontAlign> &other);
+    Quaternion(const Quaternion& other);
+    explicit Quaternion(const Eigen::Quaternion<double, Eigen::DontAlign>& other);
 
-    Quaternion &operator=(const Quaternion &other);
-    Quaternion &operator=(const Eigen::Quaternion<double, Eigen::DontAlign> &other);
+    Quaternion& operator=(const Quaternion& other);
+    Quaternion& operator=(const Eigen::Quaternion<double, Eigen::DontAlign>& other);
 
-    Quaternion(const double &w, const double &x,
-               const double &y, const double &z);
+    Quaternion(const double& w, const double& x, const double& y, const double& z);
 
-    Quaternion(double &w, double &x,
-               double &y, double &z);
+    Quaternion(double& w, double& x, double& y, double& z);
 
-    Quaternion(const Eigen::Vector3d &vector, double angle_radians);
+    Quaternion(const Eigen::Vector3d& vector, double angle_radians);
 
     /*! \brief set quaternion from euler angles */
     Quaternion(double roll, double pitch, double yaw);
@@ -73,7 +71,7 @@ class Quaternion : public Eigen::Quaternion<double, Eigen::DontAlign> {
     /*! \brief set the quaternion to rotate an object around the given vector
      * at given angle
      */
-    void set(const Eigen::Vector3d &vector, double angle_radians);
+    void set(const Eigen::Vector3d& vector, double angle_radians);
 
     /*! \brief set quaternion from euler angles */
     void set(double roll, double pitch, double yaw);
@@ -102,20 +100,20 @@ class Quaternion : public Eigen::Quaternion<double, Eigen::DontAlign> {
      * given by RotationAngle(). This can also be used to convert a vector in
      * the local frame to a vector in the global frame.
      */
-    Eigen::Vector3d rotate(const Eigen::Vector3d &vec) const;
+    Eigen::Vector3d rotate(const Eigen::Vector3d& vec) const;
 
     /*! \brief rotate the input vector around the vec axis by an angle
      * given by RotationAngle(). This can also be used to convert a vector in
      * the local frame to a vector in the global frame.
      */
-    Eigen::Vector3d rotate_reverse(const Eigen::Vector3d &vec) const;
+    Eigen::Vector3d rotate_reverse(const Eigen::Vector3d& vec) const;
     ///@}
 
     uint8_t output_precision = 2;
     friend std::ostream& operator<<(std::ostream& os, const Quaternion& q);
 };
 
-scrimmage::Quaternion operator*(const scrimmage::Quaternion &q1, const scrimmage::Quaternion &q2);
+scrimmage::Quaternion operator*(const scrimmage::Quaternion& q1, const scrimmage::Quaternion& q2);
 
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_MATH_QUATERNION_H_
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_MATH_QUATERNION_H_
diff --git a/include/scrimmage/math/State.h b/include/scrimmage/math/State.h
index db7a51de02..13998b79c5 100644
--- a/include/scrimmage/math/State.h
+++ b/include/scrimmage/math/State.h
@@ -33,12 +33,11 @@
 #ifndef INCLUDE_SCRIMMAGE_MATH_STATE_H_
 #define INCLUDE_SCRIMMAGE_MATH_STATE_H_
 
-#include <Eigen/Dense>
-
 #include <scrimmage/math/Quaternion.h>
 
-#include <memory>
+#include <Eigen/Dense>
 #include <iosfwd>
+#include <memory>
 #include <typeinfo>
 
 namespace scrimmage {
@@ -46,40 +45,39 @@ namespace scrimmage {
 class State {
  public:
     State();
-    State(Eigen::Vector3d _pos, Eigen::Vector3d _vel,
-          Eigen::Vector3d _ang_vel, Quaternion _quat);
+    State(Eigen::Vector3d _pos, Eigen::Vector3d _vel, Eigen::Vector3d _ang_vel, Quaternion _quat);
 
     virtual ~State();
 
     // Position is in local level coordinate system (East/North/Up)
-    Eigen::Vector3d &pos();
-    Eigen::Vector3d &vel();
-    Eigen::Vector3d &ang_vel();
-    const Eigen::Vector3d &pos() const;
-    const Eigen::Vector3d &vel() const;
-    const Eigen::Vector3d &ang_vel() const;
+    Eigen::Vector3d& pos();
+    Eigen::Vector3d& vel();
+    Eigen::Vector3d& ang_vel();
+    const Eigen::Vector3d& pos() const;
+    const Eigen::Vector3d& vel() const;
+    const Eigen::Vector3d& ang_vel() const;
 
     // Converts between local level (East/North/Up) and body (Nose/Left/Up)
-    Quaternion &quat();
-    const Quaternion &quat() const;
+    Quaternion& quat();
+    const Quaternion& quat() const;
 
     // for backwards compatibility
-    const Eigen::Vector3d &pos_const() const;
-    const Eigen::Vector3d &vel_const() const;
-    const Eigen::Vector3d &ang_vel_const() const;
-    const Quaternion &quat_const() const;
-    void set_pos(const Eigen::Vector3d &pos);
-    void set_vel(const Eigen::Vector3d &vel);
-    void set_ang_vel(const Eigen::Vector3d &ang_vel);
-    void set_quat(const Quaternion &quat);
+    const Eigen::Vector3d& pos_const() const;
+    const Eigen::Vector3d& vel_const() const;
+    const Eigen::Vector3d& ang_vel_const() const;
+    const Quaternion& quat_const() const;
+    void set_pos(const Eigen::Vector3d& pos);
+    void set_vel(const Eigen::Vector3d& vel);
+    void set_ang_vel(const Eigen::Vector3d& ang_vel);
+    void set_quat(const Quaternion& quat);
 
     /*! \brief Returns true if other state is in field-of-view */
-    bool InFieldOfView(State &other, double fov_width, double fov_height) const;
+    bool InFieldOfView(State& other, double fov_width, double fov_height) const;
 
     /*! \brief convert the relative position to the local frame (the output
      * vector will point to the other state)
      */
-    Eigen::Vector3d rel_pos_local_frame(Eigen::Vector3d &other) const;
+    Eigen::Vector3d rel_pos_local_frame(Eigen::Vector3d& other) const;
 
     /*! \brief return position offset by trailing_distance in the direction of
      * velocity or orientation
@@ -89,7 +87,7 @@ class State {
     /*! \brief returns the vector extending forward */
     Eigen::Vector3d orient_global_frame() const;
 
-    double rel_az(const Eigen::Vector3d &other) const;
+    double rel_az(const Eigen::Vector3d& other) const;
 
     Eigen::Matrix4d tf_matrix(bool enable_translate = true);
 
@@ -98,12 +96,10 @@ class State {
 
     /*! \brief Downcast a scrimmage::State to a subclassed type, returing nullptr if not successful
      */
-    template <class T,
-              class = std::enable_if_t<
-                !std::is_same<T, scrimmage::State>::value &&
-                std::is_base_of<scrimmage::State, T>::value, void>>
+    template <class T, class = std::enable_if_t<!std::is_same<T, scrimmage::State>::value
+                                                    && std::is_base_of<scrimmage::State, T>::value,
+                                                void>>
     static std::shared_ptr<T> cast(std::shared_ptr<scrimmage::State> state) {
-
         std::shared_ptr<T> result = std::dynamic_pointer_cast<T>(state);
         try {
             if (typeid(*result).name()) {
@@ -111,7 +107,7 @@ class State {
                 // warning. typeid will throw an exception if the underlying object
                 // is only of the base-type and not the subclassed type.
             }
-        } catch (const std::bad_typeid &e) {
+        } catch (const std::bad_typeid& e) {
             return nullptr;
         }
         return std::shared_ptr<T>(result);
@@ -129,6 +125,6 @@ class State {
 
 using StatePtr = std::shared_ptr<State>;
 
-} // namespace scrimmage
+}  // namespace scrimmage
 
 #endif  // INCLUDE_SCRIMMAGE_MATH_STATE_H_
diff --git a/include/scrimmage/math/StateWithCovariance.h b/include/scrimmage/math/StateWithCovariance.h
index 42cd31950b..4b7f2effe0 100644
--- a/include/scrimmage/math/StateWithCovariance.h
+++ b/include/scrimmage/math/StateWithCovariance.h
@@ -41,14 +41,12 @@ namespace scrimmage {
 class StateWithCovariance : public scrimmage::State {
  public:
     StateWithCovariance();
-    explicit StateWithCovariance(const scrimmage::State &state);
-    StateWithCovariance(const scrimmage::State &state,
-                        const int &cov_num_rows,
-                        const int &cov_num_cols,
-                        const double &cov_diag);
+    explicit StateWithCovariance(const scrimmage::State& state);
+    StateWithCovariance(const scrimmage::State& state, const int& cov_num_rows,
+                        const int& cov_num_cols, const double& cov_diag);
 
-    void set_covariance(const Eigen::MatrixXd &covariance);
-    const Eigen::MatrixXd &covariance();
+    void set_covariance(const Eigen::MatrixXd& covariance);
+    const Eigen::MatrixXd& covariance();
 
  protected:
     Eigen::MatrixXd covariance_;
diff --git a/include/scrimmage/metrics/Metrics.h b/include/scrimmage/metrics/Metrics.h
index ee8b8dd05a..9026bd4d9c 100644
--- a/include/scrimmage/metrics/Metrics.h
+++ b/include/scrimmage/metrics/Metrics.h
@@ -34,10 +34,10 @@
 #define INCLUDE_SCRIMMAGE_METRICS_METRICS_H_
 #include <scrimmage/entity/EntityPlugin.h>
 
-#include <map>
 #include <list>
-#include <string>
+#include <map>
 #include <memory>
+#include <string>
 
 namespace scrimmage {
 
@@ -47,7 +47,7 @@ class Metrics : public EntityPlugin {
     virtual ~Metrics();
 
     virtual void init();
-    virtual void init(std::map<std::string, std::string> &params);
+    virtual void init(std::map<std::string, std::string>& params);
     virtual bool step_metrics(double t, double dt);
 
     virtual void calc_team_scores();
@@ -57,13 +57,15 @@ class Metrics : public EntityPlugin {
     // Value 1: Map of..
     //                   Key 2  : Header string
     //                   Value 2: Metric value
-    virtual std::map<int, std::map<std::string, double>> &team_metrics();
+    virtual std::map<int, std::map<std::string, double>>& team_metrics();
 
-    virtual std::list<std::string> &headers();
+    virtual std::list<std::string>& headers();
 
-    virtual std::map<int, double> & team_scores();
+    virtual std::map<int, double>& team_scores();
 
-    bool get_print_team_summary() {return print_team_summary_;}
+    bool get_print_team_summary() {
+        return print_team_summary_;
+    }
 
  protected:
     std::string weights_file_;
@@ -75,5 +77,5 @@ class Metrics : public EntityPlugin {
 
 using MetricsPtr = std::shared_ptr<Metrics>;
 
-} // namespace scrimmage
+}  // namespace scrimmage
 #endif  // INCLUDE_SCRIMMAGE_METRICS_METRICS_H_
diff --git a/include/scrimmage/motion/Controller.h b/include/scrimmage/motion/Controller.h
index d97a3da219..8d5d1e97dc 100644
--- a/include/scrimmage/motion/Controller.h
+++ b/include/scrimmage/motion/Controller.h
@@ -32,21 +32,28 @@
 
 #ifndef INCLUDE_SCRIMMAGE_MOTION_CONTROLLER_H_
 #define INCLUDE_SCRIMMAGE_MOTION_CONTROLLER_H_
-#include <scrimmage/fwd_decl.h>
 #include <scrimmage/entity/EntityPlugin.h>
+#include <scrimmage/fwd_decl.h>
 
-#include <memory>
 #include <map>
+#include <memory>
 #include <string>
 
 namespace scrimmage {
 
 class Controller : public EntityPlugin {
  public:
-    virtual void init(std::map<std::string, std::string> &/*params*/) {}
-    virtual bool step(double /*t*/, double /*dt*/) {return true;}
-    inline void set_state(StatePtr &state) {state_ = state;}
-    inline void set_desired_state(StatePtr &desired_state) {desired_state_ = desired_state;}
+    virtual void init(std::map<std::string, std::string>& /*params*/) {
+    }
+    virtual bool step(double /*t*/, double /*dt*/) {
+        return true;
+    }
+    inline void set_state(StatePtr& state) {
+        state_ = state;
+    }
+    inline void set_desired_state(StatePtr& desired_state) {
+        desired_state_ = desired_state;
+    }
     void close(double /*t*/) override {
         state_ = nullptr;
         desired_state_ = nullptr;
@@ -59,5 +66,5 @@ class Controller : public EntityPlugin {
 
 using ControllerPtr = std::shared_ptr<Controller>;
 
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_MOTION_CONTROLLER_H_
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_MOTION_CONTROLLER_H_
diff --git a/include/scrimmage/motion/MotionModel.h b/include/scrimmage/motion/MotionModel.h
index 47bb39477a..a68389c477 100644
--- a/include/scrimmage/motion/MotionModel.h
+++ b/include/scrimmage/motion/MotionModel.h
@@ -37,7 +37,6 @@
 #include <scrimmage/fwd_decl.h>
 
 #include <Eigen/Dense>
-
 #include <map>
 #include <memory>
 #include <string>
@@ -52,25 +51,33 @@ class MotionModel : public EntityPlugin {
     MotionModel();
     std::string type() override;
 
-    virtual bool init(std::map<std::string, std::string> &info,
-                      std::map<std::string, std::string> &params);
+    virtual bool init(std::map<std::string, std::string>& info,
+                      std::map<std::string, std::string>& params);
 
     virtual bool step(double time, double dt);
     virtual bool posthumous(double t);
-    virtual StatePtr &state();
-    virtual void set_state(StatePtr &state);
-    virtual void teleport(StatePtr &state);
-    virtual void set_external_force(const Eigen::Vector3d &force);
-    virtual void set_external_moment(const Eigen::Vector3d &moment);
-    virtual void set_mass(double mass) { mass_ = mass; }
-    virtual double mass() { return mass_; }
-    virtual double gravity_magnitude() { return g_; }
-    virtual std::vector<double> &full_state_vector() { return x_; }
+    virtual StatePtr& state();
+    virtual void set_state(StatePtr& state);
+    virtual void teleport(StatePtr& state);
+    virtual void set_external_force(const Eigen::Vector3d& force);
+    virtual void set_external_moment(const Eigen::Vector3d& moment);
+    virtual void set_mass(double mass) {
+        mass_ = mass;
+    }
+    virtual double mass() {
+        return mass_;
+    }
+    virtual double gravity_magnitude() {
+        return g_;
+    }
+    virtual std::vector<double>& full_state_vector() {
+        return x_;
+    }
     void close(double t) override;
 
  protected:
     void ode_step(double dt);
-    virtual void model(const vector_t &x, vector_t &dxdt, double t);
+    virtual void model(const vector_t& x, vector_t& dxdt, double t);
 
     StatePtr state_;
     vector_t x_;
diff --git a/include/scrimmage/network/Interface.h b/include/scrimmage/network/Interface.h
index aea3357a50..cdfa83138a 100644
--- a/include/scrimmage/network/Interface.h
+++ b/include/scrimmage/network/Interface.h
@@ -36,33 +36,35 @@
 #include <scrimmage/fwd_decl.h>
 #include <scrimmage/network/ScrimmageServiceImpl.h>
 #include <scrimmage/proto/Frame.pb.h>
-#include <scrimmage/proto/Visual.pb.h>
 #include <scrimmage/proto/GUIControl.pb.h>
 #include <scrimmage/proto/Shape.pb.h>
+#include <scrimmage/proto/Visual.pb.h>
 
 #if ENABLE_GRPC == 1
-#include <scrimmage/proto/Scrimmage.grpc.pb.h>
 #include <grpc++/grpc++.h>
+#include <scrimmage/proto/Scrimmage.grpc.pb.h>
 #endif
 
 #include <list>
-#include <mutex> // NOLINT
-#include <string>
 #include <memory>
+#include <mutex>  // NOLINT
+#include <string>
 
 namespace scrimmage {
 class Interface {
  public:
-    typedef enum Mode {
-        shared = 0,
-        client = 1,
-        server = 2
-    } Mode_t;
-
-    void set_mode(Mode_t mode) { mode_ = mode; }
-    void set_ip(std::string &ip) { ip_ = ip; }
-    void set_port(int port) { port_ = port; }
-    bool init_network(Mode_t mode, const std::string &ip, int port);
+    typedef enum Mode { shared = 0, client = 1, server = 2 } Mode_t;
+
+    void set_mode(Mode_t mode) {
+        mode_ = mode;
+    }
+    void set_ip(std::string& ip) {
+        ip_ = ip;
+    }
+    void set_port(int port) {
+        port_ = port;
+    }
+    bool init_network(Mode_t mode, const std::string& ip, int port);
 
     bool frames_update(double t);
     bool utm_terrain_update();
@@ -73,28 +75,28 @@ class Interface {
     bool shapes_update();
 
     // SimControl to GUI
-    bool send_frame(double time, ContactMapPtr &contacts);
+    bool send_frame(double time, ContactMapPtr& contacts);
 
-    bool send_frame(std::shared_ptr<scrimmage_proto::Frame> &frame);
+    bool send_frame(std::shared_ptr<scrimmage_proto::Frame>& frame);
 
-    bool send_utm_terrain(std::shared_ptr<scrimmage_proto::UTMTerrain> &utm_terrain);
-    bool send_contact_visual(std::shared_ptr<scrimmage_proto::ContactVisual> &cv);
+    bool send_utm_terrain(std::shared_ptr<scrimmage_proto::UTMTerrain>& utm_terrain);
+    bool send_contact_visual(std::shared_ptr<scrimmage_proto::ContactVisual>& cv);
 
-    bool push_frame(std::shared_ptr<scrimmage_proto::Frame> &frame);
-    bool push_utm_terrain(std::shared_ptr<scrimmage_proto::UTMTerrain> &utm_terrain);
-    bool push_contact_visual(std::shared_ptr<scrimmage_proto::ContactVisual> &cv);
+    bool push_frame(std::shared_ptr<scrimmage_proto::Frame>& frame);
+    bool push_utm_terrain(std::shared_ptr<scrimmage_proto::UTMTerrain>& utm_terrain);
+    bool push_contact_visual(std::shared_ptr<scrimmage_proto::ContactVisual>& cv);
 
     // GUI to SimControl
-    bool send_gui_msg(scrimmage_proto::GUIMsg &gui_msg);
-    bool push_gui_msg(scrimmage_proto::GUIMsg &gui_msg);
-    bool send_world_point_clicked_msg(scrimmage_proto::WorldPointClicked &msg);
-    bool push_world_point_clicked_msg(scrimmage_proto::WorldPointClicked &msg);
+    bool send_gui_msg(scrimmage_proto::GUIMsg& gui_msg);
+    bool push_gui_msg(scrimmage_proto::GUIMsg& gui_msg);
+    bool send_world_point_clicked_msg(scrimmage_proto::WorldPointClicked& msg);
+    bool push_world_point_clicked_msg(scrimmage_proto::WorldPointClicked& msg);
 
-    bool send_sim_info(scrimmage_proto::SimInfo &sim_info);
-    bool push_sim_info(scrimmage_proto::SimInfo &sim_info);
+    bool send_sim_info(scrimmage_proto::SimInfo& sim_info);
+    bool push_sim_info(scrimmage_proto::SimInfo& sim_info);
 
-    bool send_shapes(scrimmage_proto::Shapes &shapes);
-    bool push_shapes(scrimmage_proto::Shapes &shapes);
+    bool send_shapes(scrimmage_proto::Shapes& shapes);
+    bool push_shapes(scrimmage_proto::Shapes& shapes);
 
     std::mutex frames_mutex;
     std::mutex utm_terrain_mutex;
@@ -104,26 +106,33 @@ class Interface {
     std::mutex shapes_mutex;
     std::mutex contact_visual_mutex;
 
-    std::list<std::shared_ptr<scrimmage_proto::Frame> > & frames()
-    { return frames_list_; }
+    std::list<std::shared_ptr<scrimmage_proto::Frame> >& frames() {
+        return frames_list_;
+    }
 
-    std::list<std::shared_ptr<scrimmage_proto::UTMTerrain> > & utm_terrain()
-    { return utm_terrain_list_; }
+    std::list<std::shared_ptr<scrimmage_proto::UTMTerrain> >& utm_terrain() {
+        return utm_terrain_list_;
+    }
 
-    std::list<std::shared_ptr<scrimmage_proto::ContactVisual> > & contact_visual()
-    { return contact_visual_list_; }
+    std::list<std::shared_ptr<scrimmage_proto::ContactVisual> >& contact_visual() {
+        return contact_visual_list_;
+    }
 
-    std::list<scrimmage_proto::GUIMsg> & gui_msg()
-    { return gui_msg_list_; }
+    std::list<scrimmage_proto::GUIMsg>& gui_msg() {
+        return gui_msg_list_;
+    }
 
-    std::list<scrimmage_proto::WorldPointClicked> & world_point_clicked_msg()
-    { return world_point_clicked_msg_list_; }
+    std::list<scrimmage_proto::WorldPointClicked>& world_point_clicked_msg() {
+        return world_point_clicked_msg_list_;
+    }
 
-    std::list<scrimmage_proto::SimInfo> & sim_info()
-    { return sim_info_list_; }
+    std::list<scrimmage_proto::SimInfo>& sim_info() {
+        return sim_info_list_;
+    }
 
-    std::list<scrimmage_proto::Shapes> & shapes()
-    { return shapes_list_; }
+    std::list<scrimmage_proto::Shapes>& shapes() {
+        return shapes_list_;
+    }
 
     void send_cached();
     bool check_ready();
@@ -160,5 +169,5 @@ class Interface {
     std::list<std::shared_ptr<scrimmage_proto::ContactVisual> > contact_visual_cache_;
 };
 using InterfacePtr = std::shared_ptr<Interface>;
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_NETWORK_INTERFACE_H_
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_NETWORK_INTERFACE_H_
diff --git a/include/scrimmage/network/ScrimmageServiceImpl.h b/include/scrimmage/network/ScrimmageServiceImpl.h
index f880b19444..1a9b56fce4 100644
--- a/include/scrimmage/network/ScrimmageServiceImpl.h
+++ b/include/scrimmage/network/ScrimmageServiceImpl.h
@@ -35,16 +35,14 @@
 
 #if ENABLE_GRPC == 1
 
+#include <google/protobuf/empty.pb.h>
 #include <scrimmage/proto/Frame.pb.h>
-#include <scrimmage/proto/Visual.pb.h>
 #include <scrimmage/proto/Scrimmage.grpc.pb.h>
+#include <scrimmage/proto/Visual.pb.h>
 
-#include <google/protobuf/empty.pb.h>
-
+#include <future>  // NOLINT
 #include <list>
-#include <mutex> // NOLINT
-#include <future> // NOLINT
-
+#include <mutex>  // NOLINT
 
 namespace scrimmage {
 
@@ -52,12 +50,13 @@ class Interface;
 
 class ScrimmageServiceImpl final : public scrimmage_proto::ScrimmageService::Service {
  public:
-    explicit ScrimmageServiceImpl(Interface *interface);
+    explicit ScrimmageServiceImpl(Interface* interface);
 
     grpc::Status SendFrame(grpc::ServerContext* context, const scrimmage_proto::Frame* frame,
                            scrimmage_proto::BlankReply* reply) override;
 
-    grpc::Status SendUTMTerrain(grpc::ServerContext* context, const scrimmage_proto::UTMTerrain* terrain,
+    grpc::Status SendUTMTerrain(grpc::ServerContext* context,
+                                const scrimmage_proto::UTMTerrain* terrain,
                                 scrimmage_proto::BlankReply* reply) override;
 
     grpc::Status SendSimInfo(grpc::ServerContext* context, const scrimmage_proto::SimInfo* sim_info,
@@ -70,21 +69,19 @@ class ScrimmageServiceImpl final : public scrimmage_proto::ScrimmageService::Ser
                                    const scrimmage_proto::ContactVisual* contact_visual,
                                    scrimmage_proto::BlankReply* reply) override;
 
-    grpc::Status SendShapes(grpc::ServerContext* context,
-                            const scrimmage_proto::Shapes* shape,
+    grpc::Status SendShapes(grpc::ServerContext* context, const scrimmage_proto::Shapes* shape,
                             scrimmage_proto::BlankReply* reply) override;
 
-    grpc::Status Ready(grpc::ServerContext* context,
-                       const google::protobuf::Empty* shape,
+    grpc::Status Ready(grpc::ServerContext* context, const google::protobuf::Empty* shape,
                        scrimmage_proto::BlankReply* reply) override;
 
     std::promise<void> exit_requested;
 
  protected:
-    Interface * interface_;
+    Interface* interface_;
 };
-} // namespace scrimmage
+}  // namespace scrimmage
 
-#endif // ENABLE_GRPC == 1
+#endif  // ENABLE_GRPC == 1
 
-#endif // INCLUDE_SCRIMMAGE_NETWORK_SCRIMMAGESERVICEIMPL_H_
+#endif  // INCLUDE_SCRIMMAGE_NETWORK_SCRIMMAGESERVICEIMPL_H_
diff --git a/include/scrimmage/parse/ConfigParse.h b/include/scrimmage/parse/ConfigParse.h
index f4fdea8283..bdaa7fcd6c 100644
--- a/include/scrimmage/parse/ConfigParse.h
+++ b/include/scrimmage/parse/ConfigParse.h
@@ -34,11 +34,12 @@
 #define INCLUDE_SCRIMMAGE_PARSE_CONFIGPARSE_H_
 
 #include <map>
-#include <vector>
 #include <string>
+#include <vector>
 
 namespace rapidxml {
-template <class T> class xml_node;
+template <class T>
+class xml_node;
 }
 
 namespace scrimmage {
@@ -49,10 +50,9 @@ class ConfigParse {
  public:
     ConfigParse();
     void set_required(std::string node_name);
-    bool parse(const std::map<std::string, std::string> &overrides,
-        std::string filename, std::string env_var, FileSearch &file_search,
-        bool verbose = false);
-    std::map<std::string, std::string> & params();
+    bool parse(const std::map<std::string, std::string>& overrides, std::string filename,
+               std::string env_var, FileSearch& file_search, bool verbose = false);
+    std::map<std::string, std::string>& params();
     std::string filename();
     std::string directory();
     std::string extension();
@@ -66,10 +66,9 @@ class ConfigParse {
     std::vector<std::string> required_;
     std::string filename_;
 
-    void recursive_params(rapidxml::xml_node<char> *root,
-        const std::map<std::string, std::string> &overrides,
-        std::map<std::string, std::string> &params,
-        const std::string &prev);
+    void recursive_params(rapidxml::xml_node<char>* root,
+                          const std::map<std::string, std::string>& overrides,
+                          std::map<std::string, std::string>& params, const std::string& prev);
 };
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PARSE_CONFIGPARSE_H_
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PARSE_CONFIGPARSE_H_
diff --git a/include/scrimmage/parse/MissionParse.h b/include/scrimmage/parse/MissionParse.h
index 7ffd7ac023..bcf5ce115a 100644
--- a/include/scrimmage/parse/MissionParse.h
+++ b/include/scrimmage/parse/MissionParse.h
@@ -38,7 +38,6 @@
 #include <scrimmage/proto/Visual.pb.h>
 
 #include <Eigen/Dense>
-
 #include <list>
 #include <map>
 #include <memory>
@@ -81,9 +80,9 @@ struct GenerateInfo {
 class MissionParse {
  public:
     bool create_log_dir();
-    void set_overrides(const std::string &overrides);
-    bool parse(const std::string &filename);
-    bool write(const std::string &filename);
+    void set_overrides(const std::string& overrides);
+    bool parse(const std::string& filename);
+    bool write(const std::string& filename);
 
     double t0();
     double tend();
@@ -92,29 +91,29 @@ class MissionParse {
     double time_warp();
     void set_time_warp(double warp);
     bool start_paused();
-    const bool &full_screen();
-    const unsigned &window_width();
-    const unsigned &window_height();
+    const bool& full_screen();
+    const unsigned& window_width();
+    const unsigned& window_height();
 
-    void set_dt(const double &dt);
+    void set_dt(const double& dt);
 
     bool parse_terrain();
 
-    scrimmage_proto::Color &background_color();
+    scrimmage_proto::Color& background_color();
 
-    void set_log_dir(const std::string &log_dir);
+    void set_log_dir(const std::string& log_dir);
     std::string log_dir();
     std::string root_log_dir();
 
-    std::map<int, AttributeMap> &entity_attributes();
+    std::map<int, AttributeMap>& entity_attributes();
 
-    std::map<int, std::map<std::string, std::string>> &entity_params();
+    std::map<int, std::map<std::string, std::string>>& entity_params();
 
-    std::map<int, int> &ent_id_to_block_id();
+    std::map<int, int>& ent_id_to_block_id();
 
-    EntityDesc_t &entity_descriptions();
+    EntityDesc_t& entity_descriptions();
 
-    std::map<std::string, int> &entity_tag_to_id();
+    std::map<std::string, int>& entity_tag_to_id();
 
     bool enable_gui();
     void set_enable_gui(bool enable);
@@ -122,39 +121,39 @@ class MissionParse {
     void set_network_gui(bool enable);
     void set_start_paused(bool paused);
 
-    AttributeMap &attributes();
-    std::map<std::string, std::string> &params();
+    AttributeMap& attributes();
+    std::map<std::string, std::string>& params();
 
     double longitude_origin();
     double latitude_origin();
     double altitude_origin();
 
-    void set_lat_lon_alt_origin(const double &lat, const double &lon, const double &alt);
+    void set_lat_lon_alt_origin(const double& lat, const double& lon, const double& alt);
 
-    std::map<int, TeamInfo> &team_info();
+    std::map<int, TeamInfo>& team_info();
 
     void set_task_number(int task_num);
     void set_job_number(int job_num);
 
     std::list<std::string> entity_interactions();
-    std::list<std::string> &network_names();
+    std::list<std::string>& network_names();
 
     std::list<std::string> metrics();
 
-    std::map<int, GenerateInfo> &gen_info();
+    std::map<int, GenerateInfo>& gen_info();
 
-    std::map<int, std::vector<double>> &next_gen_times();
+    std::map<int, std::vector<double>>& next_gen_times();
 
     std::shared_ptr<GeographicLib::LocalCartesian> projection();
 
-    std::shared_ptr<scrimmage_proto::UTMTerrain> &utm_terrain();
+    std::shared_ptr<scrimmage_proto::UTMTerrain>& utm_terrain();
 
     std::string get_mission_filename();
 
     bool get_no_bin_logging();
 
     bool output_required();
-    bool output_type_required(const std::string &output_type);
+    bool output_type_required(const std::string& output_type);
 
  protected:
     std::string mission_filename_ = "";
@@ -175,7 +174,7 @@ class MissionParse {
 
     bool no_bin_logging_ = false;
 
-    bool read_file_content(const std::string &filename);
+    bool read_file_content(const std::string& filename);
     std::string replace_overrides(std::string str);
 
     template <class Parser>
@@ -237,8 +236,8 @@ class MissionParse {
     std::set<std::string> output_types_;
 
     // Set of all possible output types
-    const std::set<std::string> possible_output_types_ = {
-        "frames", "summary", "git_commits", "mission", "seed", "runtime"};
+    const std::set<std::string> possible_output_types_ = {"frames",  "summary", "git_commits",
+                                                          "mission", "seed",    "runtime"};
 };
 using MissionParsePtr = std::shared_ptr<MissionParse>;
 }  // namespace scrimmage
diff --git a/include/scrimmage/parse/ParseUtils.h b/include/scrimmage/parse/ParseUtils.h
index af1cfb0542..18fe5499b7 100644
--- a/include/scrimmage/parse/ParseUtils.h
+++ b/include/scrimmage/parse/ParseUtils.h
@@ -33,18 +33,17 @@
 #ifndef INCLUDE_SCRIMMAGE_PARSE_PARSEUTILS_H_
 #define INCLUDE_SCRIMMAGE_PARSE_PARSEUTILS_H_
 #include <Eigen/Dense>
-
+#include <list>
 #include <map>
+#include <memory>
 #include <set>
-#include <vector>
 #include <string>
-#include <memory>
-#include<list>
+#include <vector>
 
 namespace scrimmage_proto {
 class UTMTerrain;
 class ContactVisual;
-}
+}  // namespace scrimmage_proto
 
 namespace scrimmage {
 
@@ -57,8 +56,7 @@ std::string expand_user(std::string path);
 bool str2bool(std::string str);
 
 template <class T>
-std::enable_if_t<!std::is_pointer<T>::value, T>
-convert(const std::string &str) {
+std::enable_if_t<!std::is_pointer<T>::value, T> convert(const std::string& str) {
     T num;
     if (!(std::istringstream(str) >> num)) {
         num = 0;
@@ -67,34 +65,41 @@ convert(const std::string &str) {
 }
 
 template <class T>
-std::enable_if_t<std::is_pointer<T>::value, T>
-convert(const std::string &str) {
+std::enable_if_t<std::is_pointer<T>::value, T> convert(const std::string& str) {
     return nullptr;
 }
 
-template <> inline bool convert<bool>(const std::string &str) {return str2bool(str);}
-template <> inline const char* convert<const char *>(const std::string &str) {return str.c_str();}
-template <> inline std::string convert<std::string>(const std::string &str) {return str;}
+template <>
+inline bool convert<bool>(const std::string& str) {
+    return str2bool(str);
+}
+template <>
+inline const char* convert<const char*>(const std::string& str) {
+    return str.c_str();
+}
+template <>
+inline std::string convert<std::string>(const std::string& str) {
+    return str;
+}
 
 template <class T1, class T2 = T1>
-    T2 get(const std::string &key, const std::map<std::string, std::string> &map, T1 default_val) {
+T2 get(const std::string& key, const std::map<std::string, std::string>& map, T1 default_val) {
     auto it = map.find(key);
     return it == map.end() ? default_val : convert<T2>(it->second);
 }
 
 // a wrapper around boost so that boost does not have to be included
-void split(std::vector<std::string> &tokens, const std::string &str,
-           const std::string &delims);
+void split(std::vector<std::string>& tokens, const std::string& str, const std::string& delims);
 
-std::string remove_whitespace(const std::string &str);
+std::string remove_whitespace(const std::string& str);
 
 template <typename T>
-T str2container(const std::string &str, const std::string &delims) {
+T str2container(const std::string& str, const std::string& delims) {
     T out;
     std::vector<std::string> tokens;
     split(tokens, str, delims);
 
-    for (std::string &t : tokens) {
+    for (std::string& t : tokens) {
         if (t.length() > 0) {
             out.insert(out.end(), convert<typename T::value_type>(t));
         }
@@ -103,8 +108,7 @@ T str2container(const std::string &str, const std::string &delims) {
 }
 
 template <typename T>
-bool str2container(const std::string &str, const std::string &delims,
-                   T &result, int size = -1) {
+bool str2container(const std::string& str, const std::string& delims, T& result, int size = -1) {
     T tmp = str2container<T>(str, delims);
     if (size >= 0 && (unsigned int)size != tmp.size()) {
         return false;
@@ -117,12 +121,12 @@ bool str2container(const std::string &str, const std::string &delims,
 
 template <typename T>
 // [[deprecated("str2vec is deprecated, use str2container instead")]]
-std::vector<T> str2vec(const std::string &str, const std::string &delims) {
+std::vector<T> str2vec(const std::string& str, const std::string& delims) {
     std::vector<T> out;
     std::vector<std::string> tokens;
     split(tokens, str, delims);
 
-    for (std::string &t : tokens) {
+    for (std::string& t : tokens) {
         if (t.length() > 0) {
             out.push_back(convert<T>(t));
         }
@@ -132,13 +136,13 @@ std::vector<T> str2vec(const std::string &str, const std::string &delims) {
 
 template <typename T>
 // [[deprecated("str2vec is deprecated, use str2container instead")]]
-bool str2vec(const std::string &str, const std::string &delims,
-             std::vector<T> &vec, int size = -1) {
+bool str2vec(const std::string& str, const std::string& delims, std::vector<T>& vec,
+             int size = -1) {
     std::vector<T> tmp_vec;
     std::vector<std::string> tokens;
     split(tokens, str, delims);
 
-    for (std::string &t : tokens) {
+    for (std::string& t : tokens) {
         if (t.length() > 0) {
             tmp_vec.push_back(convert<T>(t));
         }
@@ -154,56 +158,46 @@ bool str2vec(const std::string &str, const std::string &delims,
 }
 
 template <typename T>
-bool get_vec(const std::string &str,
-             const std::map<std::string, std::string> &params,
-             const std::string &delims,
-             std::vector<T> &vec,
-             int size = -1) {
+bool get_vec(const std::string& str, const std::map<std::string, std::string>& params,
+             const std::string& delims, std::vector<T>& vec, int size = -1) {
     auto it = params.find(str);
     return it == params.end() ? false : str2container(it->second, delims, vec, size);
 }
 
-bool get_vec(const std::string &str,
-             std::map<std::string, std::string> & params,
-             std::vector<std::string> &vec);
-
-Eigen::Vector3d vec2eigen(std::vector<double> &vec);
+bool get_vec(const std::string& str, std::map<std::string, std::string>& params,
+             std::vector<std::string>& vec);
 
+Eigen::Vector3d vec2eigen(std::vector<double>& vec);
 
-bool find_terrain_files(std::string terrain_name,
-                        ConfigParse &terrain_parse,
-                        std::shared_ptr<scrimmage_proto::UTMTerrain> &utm_terrain);
+bool find_terrain_files(std::string terrain_name, ConfigParse& terrain_parse,
+                        std::shared_ptr<scrimmage_proto::UTMTerrain>& utm_terrain);
 
-bool find_model_properties(std::string model_name, ConfigParse &cv_parse,
-                           FileSearch &file_search,
-                           std::map<std::string, std::string> &overrides,
-                           std::shared_ptr<scrimmage_proto::ContactVisual> &cv,
-                           bool &mesh_found, bool &texture_found);
+bool find_model_properties(std::string model_name, ConfigParse& cv_parse, FileSearch& file_search,
+                           std::map<std::string, std::string>& overrides,
+                           std::shared_ptr<scrimmage_proto::ContactVisual>& cv, bool& mesh_found,
+                           bool& texture_found);
 
-bool parse_autonomy_data(std::map<std::string, std::string> &params,
-                         std::map<std::string, std::string> &data_params);
+bool parse_autonomy_data(std::map<std::string, std::string>& params,
+                         std::map<std::string, std::string>& data_params);
 
-bool get_vec_of_vecs(const std::string &str,
-                     std::vector<std::vector<std::string>> &out,
-                     const std::string &delims = " ,");
+bool get_vec_of_vecs(const std::string& str, std::vector<std::vector<std::string>>& out,
+                     const std::string& delims = " ,");
 
-bool set_pid_gains(PID &pid, std::string str,
-                   bool is_angle = false);
+bool set_pid_gains(PID& pid, std::string str, bool is_angle = false);
 
 struct PluginOverrides {
     std::string name;
     std::map<std::string, std::string> overrides;
-    PluginOverrides(const std::string& n,
-                    std::map<std::string, std::string> &o)
-            : name(n), overrides(o) { }
+    PluginOverrides(const std::string& n, std::map<std::string, std::string>& o)
+        : name(n), overrides(o) {
+    }
 };
 
-unsigned int parse_plugin_vector(const std::string& key,
-                                 std::map<std::string, std::string> &params,
-                                 std::list<PluginOverrides> &plugin_overrides_list);
+unsigned int parse_plugin_vector(const std::string& key, std::map<std::string, std::string>& params,
+                                 std::list<PluginOverrides>& plugin_overrides_list);
 
-void remove_leading_spaces(std::string &s);
-void remove_trailing_spaces(std::string &s);
+void remove_leading_spaces(std::string& s);
+void remove_trailing_spaces(std::string& s);
 
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PARSE_PARSEUTILS_H_
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PARSE_PARSEUTILS_H_
diff --git a/include/scrimmage/parse/XMLParser/LibXML2Parser.h b/include/scrimmage/parse/XMLParser/LibXML2Parser.h
index 52adb5bad9..2f8394241d 100644
--- a/include/scrimmage/parse/XMLParser/LibXML2Parser.h
+++ b/include/scrimmage/parse/XMLParser/LibXML2Parser.h
@@ -31,10 +31,9 @@
 #ifndef INCLUDE_SCRIMMAGE_PARSE_XMLPARSER_LIBXML2PARSER_H_
 #define INCLUDE_SCRIMMAGE_PARSE_XMLPARSER_LIBXML2PARSER_H_
 
-#include <scrimmage/parse/XMLParser/XMLParser.h>
-
 #include <libxml/tree.h>
 #include <libxml/xmlreader.h>
+#include <scrimmage/parse/XMLParser/XMLParser.h>
 
 #include <string>
 #include <vector>
diff --git a/include/scrimmage/parse/XMLParser/RapidXMLParser.h b/include/scrimmage/parse/XMLParser/RapidXMLParser.h
index c6005e346d..d3a3a5ffab 100644
--- a/include/scrimmage/parse/XMLParser/RapidXMLParser.h
+++ b/include/scrimmage/parse/XMLParser/RapidXMLParser.h
@@ -34,96 +34,95 @@
 #include <scrimmage/parse/XMLParser/XMLParser.h>
 
 #include <cassert>
+#include <rapidxml/rapidxml.hpp>
 #include <string>
 #include <vector>
 
-#include <rapidxml/rapidxml.hpp>
-
 namespace scrimmage {
 
 class RapidXMLParserAttribute : public XMLParserAttribute<RapidXMLParserAttribute> {
  public:
-  explicit RapidXMLParserAttribute(rapidxml::xml_attribute<> *attribute);
+    explicit RapidXMLParserAttribute(rapidxml::xml_attribute<>* attribute);
 
-  RapidXMLParserAttribute next_attribute(const std::string &name) const;
-  RapidXMLParserAttribute next_attribute() const;
+    RapidXMLParserAttribute next_attribute(const std::string& name) const;
+    RapidXMLParserAttribute next_attribute() const;
 
-  RapidXMLParserAttribute prev_attribute(const std::string &name) const;
-  RapidXMLParserAttribute prev_attribute() const;
+    RapidXMLParserAttribute prev_attribute(const std::string& name) const;
+    RapidXMLParserAttribute prev_attribute() const;
 
-  std::string attribute_name() const;
-  std::string attribute_value() const;
+    std::string attribute_name() const;
+    std::string attribute_value() const;
 
-  bool is_valid_attribute() const;
+    bool is_valid_attribute() const;
 
-  friend bool inline operator==(const RapidXMLParserAttribute &lhs,
-                                const RapidXMLParserAttribute &rhs) {
-    return lhs.attribute_ == rhs.attribute_;
-  }
+    friend bool inline operator==(const RapidXMLParserAttribute& lhs,
+                                  const RapidXMLParserAttribute& rhs) {
+        return lhs.attribute_ == rhs.attribute_;
+    }
 
-  friend bool inline operator!=(const RapidXMLParserAttribute &lhs,
-                                const RapidXMLParserAttribute &rhs) {
-    return !(lhs == rhs);
-  }
+    friend bool inline operator!=(const RapidXMLParserAttribute& lhs,
+                                  const RapidXMLParserAttribute& rhs) {
+        return !(lhs == rhs);
+    }
 
  protected:
-  rapidxml::xml_attribute<> *attribute_;
+    rapidxml::xml_attribute<>* attribute_;
 };
 
 template <>
 struct XMLParserTraits<class RapidXMLParserNode> {
-  using child = RapidXMLParserAttribute;
+    using child = RapidXMLParserAttribute;
 };
 
 class RapidXMLParserNode : public XMLParserNode<RapidXMLParserNode> {
  public:
-  explicit RapidXMLParserNode(rapidxml::xml_node<> *node);
+    explicit RapidXMLParserNode(rapidxml::xml_node<>* node);
 
-  RapidXMLParserNode get_first_node(const std::string &name) const;
-  RapidXMLParserNode get_first_node() const;
+    RapidXMLParserNode get_first_node(const std::string& name) const;
+    RapidXMLParserNode get_first_node() const;
 
-  RapidXMLParserNode get_next_sibling(const std::string &name) const;
-  RapidXMLParserNode get_next_sibling() const;
+    RapidXMLParserNode get_next_sibling(const std::string& name) const;
+    RapidXMLParserNode get_next_sibling() const;
 
-  RapidXMLParserNode get_prev_sibling(const std::string &name) const;
-  RapidXMLParserNode get_prev_sibling() const;
+    RapidXMLParserNode get_prev_sibling(const std::string& name) const;
+    RapidXMLParserNode get_prev_sibling() const;
 
-  RapidXMLParserAttribute get_first_attribute(const std::string &name) const;
-  RapidXMLParserAttribute get_first_attribute() const;
+    RapidXMLParserAttribute get_first_attribute(const std::string& name) const;
+    RapidXMLParserAttribute get_first_attribute() const;
 
-  std::string node_name() const;
-  std::string node_value() const;
+    std::string node_name() const;
+    std::string node_value() const;
 
-  bool is_valid_node() const;
+    bool is_valid_node() const;
 
-  friend bool inline operator==(const RapidXMLParserNode &lhs, const RapidXMLParserNode &rhs) {
-    return lhs.node_ == rhs.node_;
-  }
+    friend bool inline operator==(const RapidXMLParserNode& lhs, const RapidXMLParserNode& rhs) {
+        return lhs.node_ == rhs.node_;
+    }
 
-  friend bool inline operator!=(const RapidXMLParserNode &lhs, const RapidXMLParserNode &rhs) {
-    return !(lhs == rhs);
-  }
+    friend bool inline operator!=(const RapidXMLParserNode& lhs, const RapidXMLParserNode& rhs) {
+        return !(lhs == rhs);
+    }
 
  protected:
-  rapidxml::xml_node<> *node_;
+    rapidxml::xml_node<>* node_;
 };
 
 template <>
 struct XMLParserTraits<class RapidXMLParserDocument> {
-  using child = RapidXMLParserNode;
+    using child = RapidXMLParserNode;
 };
 
 class RapidXMLParserDocument : public XMLParserDocument<RapidXMLParserDocument> {
  public:
-  bool parse_document(const std::string &filename);
-  bool parse_document(const std::vector<char> &filecontent);
+    bool parse_document(const std::string& filename);
+    bool parse_document(const std::vector<char>& filecontent);
 
-  RapidXMLParserNode find_first_node(const std::string &name) const;
-  RapidXMLParserNode find_first_node() const;
+    RapidXMLParserNode find_first_node(const std::string& name) const;
+    RapidXMLParserNode find_first_node() const;
 
  protected:
-  std::vector<char> filecontent_;
-  rapidxml::xml_document<> doc_;
+    std::vector<char> filecontent_;
+    rapidxml::xml_document<> doc_;
 };
 
 using RapidXMLParser = RapidXMLParserDocument;
diff --git a/include/scrimmage/parse/XMLParser/XMLParser.h b/include/scrimmage/parse/XMLParser/XMLParser.h
index adf5405b82..610e556ab6 100644
--- a/include/scrimmage/parse/XMLParser/XMLParser.h
+++ b/include/scrimmage/parse/XMLParser/XMLParser.h
@@ -53,331 +53,397 @@ struct XMLParserTraits;
 template <class T>
 class XMLParserAttribute {
  public:
-  class Iterator {
-    using iterator_category = std::forward_iterator_tag;
-    using difference_type = std::ptrdiff_t;
-    using value_type = T;
-    using pointer = T *;
-    using reference = T &;
-
-   public:
-    explicit Iterator(T attr)
-        : attribute_{attr} {}
-    Iterator(const Iterator &other)
-        : attribute_{other.attribute_} {}
-
-    reference operator*() { return attribute_; }
-    pointer operator->() { return &attribute_; }
-
-    Iterator &operator++() {
-      if (attribute_ != T{nullptr}) {
-        attribute_ = attribute_.next();
-        if (!attribute_.is_valid()) {
-          attribute_ = T{nullptr};
+    class Iterator {
+        using iterator_category = std::forward_iterator_tag;
+        using difference_type = std::ptrdiff_t;
+        using value_type = T;
+        using pointer = T*;
+        using reference = T&;
+
+     public:
+        explicit Iterator(T attr) : attribute_{attr} {
+        }
+        Iterator(const Iterator& other) : attribute_{other.attribute_} {
         }
-      }
-      return *this;
-    }
 
-    Iterator &operator++(int) {
-      XMLParserAttribute &old = *this;
-      ++(*this);
-      return old;
-    }
+        reference operator*() {
+            return attribute_;
+        }
+        pointer operator->() {
+            return &attribute_;
+        }
 
-    friend bool operator==(const Iterator &rhs, const Iterator &lhs) {
-      return (lhs.attribute_ == rhs.attribute_) ||
-             (!lhs.attribute_.is_valid() && !rhs.attribute_.is_valid());
-    }
+        Iterator& operator++() {
+            if (attribute_ != T{nullptr}) {
+                attribute_ = attribute_.next();
+                if (!attribute_.is_valid()) {
+                    attribute_ = T{nullptr};
+                }
+            }
+            return *this;
+        }
 
-    friend bool operator!=(const Iterator &rhs, const Iterator &lhs) { return !(lhs == rhs); }
+        Iterator& operator++(int) {
+            XMLParserAttribute& old = *this;
+            ++(*this);
+            return old;
+        }
 
-   private:
-    T attribute_;
-  };
+        friend bool operator==(const Iterator& rhs, const Iterator& lhs) {
+            return (lhs.attribute_ == rhs.attribute_)
+                   || (!lhs.attribute_.is_valid() && !rhs.attribute_.is_valid());
+        }
 
-  T next(const std::string &name) const {
-    return static_cast<const T *>(this)->next_attribute(name);
-  }
+        friend bool operator!=(const Iterator& rhs, const Iterator& lhs) {
+            return !(lhs == rhs);
+        }
 
-  T next() const { return static_cast<const T *>(this)->next_attribute(); }
+     private:
+        T attribute_;
+    };
 
-  T prev(const std::string &name) const {
-    return static_cast<const T *>(this)->prev_attribute(name);
-  }
+    T next(const std::string& name) const {
+        return static_cast<const T*>(this)->next_attribute(name);
+    }
 
-  T prev() const { return static_cast<const T *>(this)->prev_attribute(); }
+    T next() const {
+        return static_cast<const T*>(this)->next_attribute();
+    }
+
+    T prev(const std::string& name) const {
+        return static_cast<const T*>(this)->prev_attribute(name);
+    }
+
+    T prev() const {
+        return static_cast<const T*>(this)->prev_attribute();
+    }
 
-  std::string name() const { return static_cast<const T *>(this)->attribute_name(); }
+    std::string name() const {
+        return static_cast<const T*>(this)->attribute_name();
+    }
 
-  std::string value() const { return static_cast<const T *>(this)->attribute_value(); }
+    std::string value() const {
+        return static_cast<const T*>(this)->attribute_value();
+    }
 
-  bool is_valid() const { return static_cast<const T *>(this)->is_valid_attribute(); }
+    bool is_valid() const {
+        return static_cast<const T*>(this)->is_valid_attribute();
+    }
 };
 
 template <class T>
 class XMLParserNode {
  public:
-  using XMLAttribute = typename XMLParserTraits<T>::child;
-  /*
-   * Iterates over only the siblings of a node
-   */
-  class ChildIterator {
-   public:
-    using iterator_category = std::forward_iterator_tag;
-    using difference_type = std::ptrdiff_t;
-    using value_type = T;
-    using pointer = T *;
-    using reference = T &;
-
-    explicit ChildIterator(T node)
-        : node_{node} {}
-    ChildIterator(const ChildIterator &other)
-        : node_{other.node_} {}
-    ~ChildIterator() {}
-
-    reference operator*() { return node_; }
-    pointer operator->() { return &node_; }
-
-    ChildIterator &operator++() {
-      if (node_ != T{nullptr}) {
-        node_ = node_.next_sibling();
-        if (!node_.is_valid()) {
-          node_ = T{nullptr};
+    using XMLAttribute = typename XMLParserTraits<T>::child;
+    /*
+     * Iterates over only the siblings of a node
+     */
+    class ChildIterator {
+     public:
+        using iterator_category = std::forward_iterator_tag;
+        using difference_type = std::ptrdiff_t;
+        using value_type = T;
+        using pointer = T*;
+        using reference = T&;
+
+        explicit ChildIterator(T node) : node_{node} {
         }
-      }
-      return *this;
-    }
-
-    ChildIterator &operator++(int) {
-      ChildIterator &old{*this};
-      ++(*this);
-      return old;
-    }
-
-    friend bool operator==(const ChildIterator &lhs, const ChildIterator &rhs) {
-      return (lhs.node_ == rhs.node_) || (!lhs.node_.is_valid() && !rhs.node_.is_valid());
-    }
-
-    friend bool operator!=(const ChildIterator &lhs, const ChildIterator &rhs) {
-      return (lhs.node_ != rhs.node_) && (lhs.node_.is_valid() || rhs.node_.is_valid());
-    }
-
-   private:
-    T node_;
-  };
-
-  /*
-   * Iterates over every Node that has this node as a parent.
-   * Will perform an in-order walk of the tree with this parents root
-   */
-  class RecursiveIterator {
-   public:
-    using iterator_category = std::forward_iterator_tag;
-    using difference_type = std::ptrdiff_t;
-    using value_type = T;
-    using pointer = T *;
-    using reference = T &;
-
-    explicit RecursiveIterator(T node)
-        : node_{node} {}
-
-    RecursiveIterator(const RecursiveIterator &other)
-        : node_{other.node_},
-          ancestors_{other.ancestors_} {}
-    ~RecursiveIterator() {}
-
-    reference operator*() { return node_; }
-    pointer operator->() { return &node_; }
-
-    RecursiveIterator &operator++() {
-      if (node_ != T{nullptr}) {  // Avoid incrementing if this is the end node
-        T child = node_.get_first_node();
-        T sibling = node_.get_next_sibling();
-        if (child.is_valid()) {
-          ancestors_.push_back(node_);
-          node_ = child;
-        } else if (sibling.is_valid()) {
-          node_ = sibling;
-        } else {
-          // No child or sibling.
-          node_ = T{nullptr};
-          while (!ancestors_.empty() && (!node_.is_valid())) {
-            T parent = ancestors_.back();
-            ancestors_.pop_back();
-            node_ = parent.get_next_sibling();
-          }
+        ChildIterator(const ChildIterator& other) : node_{other.node_} {
         }
-        if (!node_.is_valid()) {
-          node_ = T{nullptr};
+        ~ChildIterator() {
         }
-      }
-      return *this;
-    }
 
-    RecursiveIterator &operator++(int) {
-      RecursiveIterator &old{*this};
-      ++(*this);
-      return old;
-    }
+        reference operator*() {
+            return node_;
+        }
+        pointer operator->() {
+            return &node_;
+        }
 
-    friend bool operator==(const RecursiveIterator &lhs, const RecursiveIterator &rhs) {
-      return (lhs.node_ == rhs.node_) || (!lhs.node_.is_valid() && !rhs.node_.is_valid());
-    }
+        ChildIterator& operator++() {
+            if (node_ != T{nullptr}) {
+                node_ = node_.next_sibling();
+                if (!node_.is_valid()) {
+                    node_ = T{nullptr};
+                }
+            }
+            return *this;
+        }
 
-    friend bool operator!=(const RecursiveIterator &lhs, const RecursiveIterator &rhs) {
-      return !(lhs == rhs);
-    }
+        ChildIterator& operator++(int) {
+            ChildIterator& old{*this};
+            ++(*this);
+            return old;
+        }
 
-   private:
-    bool is_valid() { return node_ != nullptr && node_->is_valid(); }
+        friend bool operator==(const ChildIterator& lhs, const ChildIterator& rhs) {
+            return (lhs.node_ == rhs.node_) || (!lhs.node_.is_valid() && !rhs.node_.is_valid());
+        }
 
-    T node_;
-    std::vector<T> ancestors_;
-  };
+        friend bool operator!=(const ChildIterator& lhs, const ChildIterator& rhs) {
+            return (lhs.node_ != rhs.node_) && (lhs.node_.is_valid() || rhs.node_.is_valid());
+        }
 
-  T first_node(const std::string &name) const {
-    return static_cast<const T *>(this)->get_first_node(name);
-  }
+     private:
+        T node_;
+    };
+
+    /*
+     * Iterates over every Node that has this node as a parent.
+     * Will perform an in-order walk of the tree with this parents root
+     */
+    class RecursiveIterator {
+     public:
+        using iterator_category = std::forward_iterator_tag;
+        using difference_type = std::ptrdiff_t;
+        using value_type = T;
+        using pointer = T*;
+        using reference = T&;
+
+        explicit RecursiveIterator(T node) : node_{node} {
+        }
 
-  T first_node() const { return static_cast<const T *>(this)->get_first_node(); }
+        RecursiveIterator(const RecursiveIterator& other)
+            : node_{other.node_}, ancestors_{other.ancestors_} {
+        }
+        ~RecursiveIterator() {
+        }
 
-  T next_sibling(const std::string &name) const {
-    return static_cast<const T *>(this)->get_next_sibling(name);
-  }
+        reference operator*() {
+            return node_;
+        }
+        pointer operator->() {
+            return &node_;
+        }
 
-  T next_sibling() const { return static_cast<const T *>(this)->get_next_sibling(); }
+        RecursiveIterator& operator++() {
+            if (node_ != T{nullptr}) {  // Avoid incrementing if this is the end node
+                T child = node_.get_first_node();
+                T sibling = node_.get_next_sibling();
+                if (child.is_valid()) {
+                    ancestors_.push_back(node_);
+                    node_ = child;
+                } else if (sibling.is_valid()) {
+                    node_ = sibling;
+                } else {
+                    // No child or sibling.
+                    node_ = T{nullptr};
+                    while (!ancestors_.empty() && (!node_.is_valid())) {
+                        T parent = ancestors_.back();
+                        ancestors_.pop_back();
+                        node_ = parent.get_next_sibling();
+                    }
+                }
+                if (!node_.is_valid()) {
+                    node_ = T{nullptr};
+                }
+            }
+            return *this;
+        }
 
-  T prev_sibling(const std::string &name) const {
-    return static_cast<const T *>(this)->get_prev_sibling(name);
-  }
+        RecursiveIterator& operator++(int) {
+            RecursiveIterator& old{*this};
+            ++(*this);
+            return old;
+        }
 
-  T prev_sibling() const { return static_cast<const T *>(this)->get_prev_sibling(); }
+        friend bool operator==(const RecursiveIterator& lhs, const RecursiveIterator& rhs) {
+            return (lhs.node_ == rhs.node_) || (!lhs.node_.is_valid() && !rhs.node_.is_valid());
+        }
 
-  XMLAttribute first_attribute(const std::string &name) const {
-    return static_cast<const T *>(this)->get_first_attribute(name);
-  }
+        friend bool operator!=(const RecursiveIterator& lhs, const RecursiveIterator& rhs) {
+            return !(lhs == rhs);
+        }
 
-  XMLAttribute first_attribute() const {
-    return static_cast<const T *>(this)->get_first_attribute();
-  }
+     private:
+        bool is_valid() {
+            return node_ != nullptr && node_->is_valid();
+        }
 
-  std::string name() const { return static_cast<const T *>(this)->node_name(); }
+        T node_;
+        std::vector<T> ancestors_;
+    };
 
-  std::string value() const { return static_cast<const T *>(this)->node_value(); }
+    T first_node(const std::string& name) const {
+        return static_cast<const T*>(this)->get_first_node(name);
+    }
 
-  ChildIterator child_begin() const { return ChildIterator{first_node()}; }
+    T first_node() const {
+        return static_cast<const T*>(this)->get_first_node();
+    }
 
-  ChildIterator child_end() const { return ChildIterator{T{nullptr}}; }
+    T next_sibling(const std::string& name) const {
+        return static_cast<const T*>(this)->get_next_sibling(name);
+    }
+
+    T next_sibling() const {
+        return static_cast<const T*>(this)->get_next_sibling();
+    }
+
+    T prev_sibling(const std::string& name) const {
+        return static_cast<const T*>(this)->get_prev_sibling(name);
+    }
+
+    T prev_sibling() const {
+        return static_cast<const T*>(this)->get_prev_sibling();
+    }
+
+    XMLAttribute first_attribute(const std::string& name) const {
+        return static_cast<const T*>(this)->get_first_attribute(name);
+    }
+
+    XMLAttribute first_attribute() const {
+        return static_cast<const T*>(this)->get_first_attribute();
+    }
+
+    std::string name() const {
+        return static_cast<const T*>(this)->node_name();
+    }
+
+    std::string value() const {
+        return static_cast<const T*>(this)->node_value();
+    }
 
-  RecursiveIterator recur_begin() const { return RecursiveIterator{first_node()}; }
+    ChildIterator child_begin() const {
+        return ChildIterator{first_node()};
+    }
+
+    ChildIterator child_end() const {
+        return ChildIterator{T{nullptr}};
+    }
+
+    RecursiveIterator recur_begin() const {
+        return RecursiveIterator{first_node()};
+    }
 
-  RecursiveIterator recur_end() const { return RecursiveIterator{T{nullptr}}; }
+    RecursiveIterator recur_end() const {
+        return RecursiveIterator{T{nullptr}};
+    }
 
-  using Attribute = typename XMLParserTraits<T>::child;
-  using AttributeIterator = typename Attribute::Iterator;
+    using Attribute = typename XMLParserTraits<T>::child;
+    using AttributeIterator = typename Attribute::Iterator;
 
-  AttributeIterator attr_begin() const { return AttributeIterator{first_attribute()}; }
+    AttributeIterator attr_begin() const {
+        return AttributeIterator{first_attribute()};
+    }
 
-  AttributeIterator attr_end() const { return AttributeIterator{Attribute{nullptr}}; }
+    AttributeIterator attr_end() const {
+        return AttributeIterator{Attribute{nullptr}};
+    }
 
-  bool is_valid() const { return static_cast<const T *>(this)->is_valid_node(); }
+    bool is_valid() const {
+        return static_cast<const T*>(this)->is_valid_node();
+    }
 
-  friend bool operator==(const XMLParserNode &lhs, const XMLParserNode &rhs) {
-    const T *lhs_ptr = static_cast<const T *>(&lhs);
-    const T *rhs_ptr = static_cast<const T *>(&rhs);
+    friend bool operator==(const XMLParserNode& lhs, const XMLParserNode& rhs) {
+        const T* lhs_ptr = static_cast<const T*>(&lhs);
+        const T* rhs_ptr = static_cast<const T*>(&rhs);
 
-    return (*lhs_ptr == *rhs_ptr) || (!lhs.is_valid() && !rhs.is_valid());
-  }
+        return (*lhs_ptr == *rhs_ptr) || (!lhs.is_valid() && !rhs.is_valid());
+    }
 
-  friend bool operator!=(const XMLParserNode &lhs, const XMLParserNode &rhs) {
-    return !(lhs == rhs);
-  }
+    friend bool operator!=(const XMLParserNode& lhs, const XMLParserNode& rhs) {
+        return !(lhs == rhs);
+    }
 };
 
 template <class T>
 class XMLParserDocument {
  public:
-  using XMLNode = typename XMLParserTraits<T>::child;
-  using NodeChildIterator = typename XMLNode::ChildIterator;
-  using NodeRecursiveIterator = typename XMLNode::RecursiveIterator;
+    using XMLNode = typename XMLParserTraits<T>::child;
+    using NodeChildIterator = typename XMLNode::ChildIterator;
+    using NodeRecursiveIterator = typename XMLNode::RecursiveIterator;
 
-  bool parse(const std::string &filename) {
-    return static_cast<T *>(this)->parse_document(filename);
-  }
+    bool parse(const std::string& filename) {
+        return static_cast<T*>(this)->parse_document(filename);
+    }
 
-  bool parse(std::vector<char> &filecontent) {
-    return static_cast<T *>(this)->parse_document(filecontent);
-  }
+    bool parse(std::vector<char>& filecontent) {
+        return static_cast<T*>(this)->parse_document(filecontent);
+    }
 
-  XMLNode first_node(const std::string &name) const {
-    return static_cast<const T *>(this)->find_first_node(name);
-  }
+    XMLNode first_node(const std::string& name) const {
+        return static_cast<const T*>(this)->find_first_node(name);
+    }
 
-  XMLNode first_node() const { return static_cast<const T *>(this)->find_first_node(); }
+    XMLNode first_node() const {
+        return static_cast<const T*>(this)->find_first_node();
+    }
 
-  NodeChildIterator begin() const { return NodeChildIterator{first_node()}; }
+    NodeChildIterator begin() const {
+        return NodeChildIterator{first_node()};
+    }
 
-  NodeChildIterator end() const { return NodeChildIterator{XMLNode{nullptr}}; }
+    NodeChildIterator end() const {
+        return NodeChildIterator{XMLNode{nullptr}};
+    }
 
-  NodeRecursiveIterator recur_begin() const { return NodeRecursiveIterator{first_node()}; }
+    NodeRecursiveIterator recur_begin() const {
+        return NodeRecursiveIterator{first_node()};
+    }
 
-  NodeRecursiveIterator recur_end() const { return NodeRecursiveIterator{XMLNode{nullptr}}; }
+    NodeRecursiveIterator recur_end() const {
+        return NodeRecursiveIterator{XMLNode{nullptr}};
+    }
 
-  void set_filename(const std::string &filename) { filename_ = filename; }
+    void set_filename(const std::string& filename) {
+        filename_ = filename;
+    }
 
-  std::string filename() const { return filename_; }
+    std::string filename() const {
+        return filename_;
+    }
 
  protected:
-  std::string filename_;
+    std::string filename_;
 };
 
 template <class T>
-std::ostream &operator<<(std::ostream &out, const XMLParserAttribute<T> &xml_attr) {
-  if (xml_attr.is_valid()) {
-    std::string name = xml_attr.name();
-    std::string value = xml_attr.value();
-    out << name << "=\"" << value << "\"";
-  }
-  return out;
+std::ostream& operator<<(std::ostream& out, const XMLParserAttribute<T>& xml_attr) {
+    if (xml_attr.is_valid()) {
+        std::string name = xml_attr.name();
+        std::string value = xml_attr.value();
+        out << name << "=\"" << value << "\"";
+    }
+    return out;
 }
 
 template <class T>
-std::ostream &operator<<(std::ostream &out, const XMLParserNode<T> &xml_node) {
-  using pos_type = typename std::ostream::pos_type;
-  if (xml_node.is_valid()) {
-    pos_type cur_pos;
-
-    std::string name = xml_node.name();
-    assert(!name.empty());
-    std::ostringstream ss;
-
-    ss << "<" << name << " ";
-    for (auto attr_it = xml_node.attr_begin(); attr_it != xml_node.attr_end(); ++attr_it) {
-      ss << *attr_it << " ";
-    }
-    cur_pos = ss.tellp();
-    assert(cur_pos != pos_type(-1));
-    ss.seekp(cur_pos - pos_type(1));
-    ss << ">";
-    auto child_it = xml_node.child_begin();
-    auto child_end = xml_node.child_end();
-    bool has_children = child_it != child_end;
-    if (has_children) {
-      for (; child_it != child_end; ++child_it) {
-        ss << "\n" << *child_it;
-      }
-    }
-    ss << "</" << name << ">";
-    out << ss.str();
-  }
-  return out;
+std::ostream& operator<<(std::ostream& out, const XMLParserNode<T>& xml_node) {
+    using pos_type = typename std::ostream::pos_type;
+    if (xml_node.is_valid()) {
+        pos_type cur_pos;
+
+        std::string name = xml_node.name();
+        assert(!name.empty());
+        std::ostringstream ss;
+
+        ss << "<" << name << " ";
+        for (auto attr_it = xml_node.attr_begin(); attr_it != xml_node.attr_end(); ++attr_it) {
+            ss << *attr_it << " ";
+        }
+        cur_pos = ss.tellp();
+        assert(cur_pos != pos_type(-1));
+        ss.seekp(cur_pos - pos_type(1));
+        ss << ">";
+        auto child_it = xml_node.child_begin();
+        auto child_end = xml_node.child_end();
+        bool has_children = child_it != child_end;
+        if (has_children) {
+            for (; child_it != child_end; ++child_it) {
+                ss << "\n" << *child_it;
+            }
+        }
+        ss << "</" << name << ">";
+        out << ss.str();
+    }
+    return out;
 }
 
 template <class T>
-std::ostream &operator<<(std::ostream &out, const XMLParserDocument<T> &xml_doc) {
-  out << xml_doc.first_node();
-  return out;
+std::ostream& operator<<(std::ostream& out, const XMLParserDocument<T>& xml_doc) {
+    out << xml_doc.first_node();
+    return out;
 }
 }  // namespace scrimmage
 
diff --git a/include/scrimmage/plugin_manager/Plugin.h b/include/scrimmage/plugin_manager/Plugin.h
index 823a024953..00622ecca0 100644
--- a/include/scrimmage/plugin_manager/Plugin.h
+++ b/include/scrimmage/plugin_manager/Plugin.h
@@ -33,8 +33,8 @@
 #ifndef INCLUDE_SCRIMMAGE_PLUGIN_MANAGER_PLUGIN_H_
 #define INCLUDE_SCRIMMAGE_PLUGIN_MANAGER_PLUGIN_H_
 
-#include <string>
 #include <memory>
+#include <string>
 
 namespace scrimmage {
 class Plugin : public std::enable_shared_from_this<Plugin> {
@@ -44,10 +44,11 @@ class Plugin : public std::enable_shared_from_this<Plugin> {
     void set_name(std::string name);
     std::string name();
     virtual std::string type();
+
  protected:
     std::string name_ = "Unset";
     bool name_set_ = false;
 };
 using PluginPtr = std::shared_ptr<Plugin>;
-} // namespace scrimmage
+}  // namespace scrimmage
 #endif  // INCLUDE_SCRIMMAGE_PLUGIN_MANAGER_PLUGIN_H_
diff --git a/include/scrimmage/plugin_manager/PluginHelper.h b/include/scrimmage/plugin_manager/PluginHelper.h
index 20f75fa131..a1a4954749 100644
--- a/include/scrimmage/plugin_manager/PluginHelper.h
+++ b/include/scrimmage/plugin_manager/PluginHelper.h
@@ -38,12 +38,12 @@
 #include <scrimmage/parse/ParseUtils.h>
 #include <scrimmage/plugin_manager/PluginManager.h>
 
+#include <iostream>
+#include <list>
 #include <map>
+#include <memory>
 #include <set>
 #include <string>
-#include <memory>
-#include <iostream>
-#include <list>
 
 using std::cout;
 using std::endl;
@@ -54,38 +54,36 @@ template <class T>
 struct PluginAndConfig {
     std::shared_ptr<T> plugin;
     ConfigParse config_parse;
-    PluginAndConfig() : plugin(nullptr) {}
-     PluginAndConfig(std::shared_ptr<T> p, ConfigParse &cp)
-             : plugin(p), config_parse(cp) { }
+    PluginAndConfig() : plugin(nullptr) {
+    }
+    PluginAndConfig(std::shared_ptr<T> p, ConfigParse& cp) : plugin(p), config_parse(cp) {
+    }
 };
 
 template <class T>
 bool load_plugins(const std::list<PluginOverrides>& plugin_names_overrides,
                   const std::string& parent_class_name,
-                  std::map<std::string, PluginAndConfig<T>> &plugins_and_configs,
+                  std::map<std::string, PluginAndConfig<T>>& plugins_and_configs,
                   const std::set<std::string>& plugin_tags = {},
                   PluginManagerPtr mgr = std::make_shared<PluginManager>(),
-                  FileSearchPtr file_search = std::make_shared<FileSearch>()
-                  ) {
+                  FileSearchPtr file_search = std::make_shared<FileSearch>()) {
     bool all_loaded = true;
-    for (const auto &plugin : plugin_names_overrides) {
+    for (const auto& plugin : plugin_names_overrides) {
         ConfigParse config_parse;
 
-        PluginStatus<T> status =
-                mgr->make_plugin<T>(parent_class_name,
-                                    plugin.name, *file_search, config_parse,
-                                    plugin.overrides, plugin_tags);
+        PluginStatus<T> status = mgr->make_plugin<T>(parent_class_name, plugin.name, *file_search,
+                                                     config_parse, plugin.overrides, plugin_tags);
 
         if (status.status == PluginStatus<T>::cast_failed) {
-            cout << "Failed to open: " << parent_class_name  << ": " << plugin.name << endl;
+            cout << "Failed to open: " << parent_class_name << ": " << plugin.name << endl;
             all_loaded = false;
         } else if (status.status == PluginStatus<T>::parse_failed) {
-            cout << "Failed to parse: " << parent_class_name  << ": " << plugin.name << endl;
+            cout << "Failed to parse: " << parent_class_name << ": " << plugin.name << endl;
             all_loaded = false;
         } else if (status.status == PluginStatus<T>::loaded) {
             auto it_name = config_parse.params().find("name");
-            std::string plugin_name = it_name == config_parse.params().end() ?
-                    plugin.name : it_name->second;
+            std::string plugin_name =
+                it_name == config_parse.params().end() ? plugin.name : it_name->second;
 
             plugins_and_configs[plugin_name] = PluginAndConfig<T>(status.plugin, config_parse);
         }
@@ -93,5 +91,5 @@ bool load_plugins(const std::list<PluginOverrides>& plugin_names_overrides,
     }
     return all_loaded;
 }
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGIN_MANAGER_PLUGINHELPER_H_
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGIN_MANAGER_PLUGINHELPER_H_
diff --git a/include/scrimmage/plugin_manager/PluginManager.h b/include/scrimmage/plugin_manager/PluginManager.h
index 7e7f7c762b..39ed2340f4 100644
--- a/include/scrimmage/plugin_manager/PluginManager.h
+++ b/include/scrimmage/plugin_manager/PluginManager.h
@@ -33,19 +33,19 @@
 #ifndef INCLUDE_SCRIMMAGE_PLUGIN_MANAGER_PLUGINMANAGER_H_
 #define INCLUDE_SCRIMMAGE_PLUGIN_MANAGER_PLUGINMANAGER_H_
 
+#include <scrimmage/common/FileSearch.h>
 #include <scrimmage/fwd_decl.h>
 #include <scrimmage/parse/ConfigParse.h>
 #include <scrimmage/parse/ParseUtils.h>
-#include <scrimmage/common/FileSearch.h>
 
 #include <iostream>
 #include <list>
 #include <map>
-#include <set>
-#include <unordered_set>
 #include <memory>
+#include <set>
 #include <string>
 #include <unordered_map>
+#include <unordered_set>
 
 #ifdef __APPLE__
 #define LIB_EXT ".dylib"
@@ -64,43 +64,39 @@ class PluginInfo {
     std::string type;
     std::string path;
     bool returned = false;
-    void * handle = nullptr;
+    void* handle = nullptr;
 };
 
 template <class T>
 class PluginStatus {
  public:
-    enum Status {parse_failed, invalid_tag, missing, loaded, cast_failed};
+    enum Status { parse_failed, invalid_tag, missing, loaded, cast_failed };
     Status status = missing;
     std::shared_ptr<T> plugin = nullptr;
 };
 
 class PluginManager {
  public:
-    typedef const char * (*plugin_name_t)();
-    typedef const char * (*plugin_type_t)();
+    typedef const char* (*plugin_name_t)();
+    typedef const char* (*plugin_type_t)();
 
     PluginManager();
 
-    void print_plugins(const std::string &plugin_type,
-                       const std::string &title,
-                       FileSearch &file_search,
-                       const std::string &env_var_name = "SCRIMMAGE_PLUGIN_PATH");
+    void print_plugins(const std::string& plugin_type, const std::string& title,
+                       FileSearch& file_search,
+                       const std::string& env_var_name = "SCRIMMAGE_PLUGIN_PATH");
     void print_returned_plugins();
 
-    void find_matching_plugins(
-        const std::string &plugin_name_so,
-        std::unordered_map<std::string, std::list<std::string>> &so_files,
-        std::list<std::string> &plugins);
+    void find_matching_plugins(const std::string& plugin_name_so,
+                               std::unordered_map<std::string, std::list<std::string>>& so_files,
+                               std::list<std::string>& plugins);
 
     template <class T>
-    PluginStatus<T> make_plugin(std::string plugin_type,
-                                const std::string &plugin_name_xml,
-                                FileSearch &file_search,
-                                ConfigParse &config_parse,
-                                const std::map<std::string, std::string> &overrides,
-                                const std::set<std::string> &plugin_tags,
-                                const std::string &env_var_name = "SCRIMMAGE_PLUGIN_PATH") {
+    PluginStatus<T> make_plugin(std::string plugin_type, const std::string& plugin_name_xml,
+                                FileSearch& file_search, ConfigParse& config_parse,
+                                const std::map<std::string, std::string>& overrides,
+                                const std::set<std::string>& plugin_tags,
+                                const std::string& env_var_name = "SCRIMMAGE_PLUGIN_PATH") {
         PluginStatus<T> status;
         std::string plugin_name = plugin_name_xml;
         auto it_orig_plugin_name = overrides.find("ORIGINAL_PLUGIN_NAME");
@@ -110,10 +106,11 @@ class PluginManager {
 
         config_parse.set_required("library");
         if (!config_parse.parse(overrides, plugin_name, env_var_name, file_search)) {
-            std::cout << "Failed to parse: " << plugin_name << ".xml for type " << plugin_type << std::endl;
+            std::cout << "Failed to parse: " << plugin_name << ".xml for type " << plugin_type
+                      << std::endl;
             std::cout << "Check that you have sourced ~/.scrimmage/setup.bash "
-                      << "(this sets the environment variable " << env_var_name << " to a directory including "
-                      << plugin_name << ".xml)" << std::endl;
+                      << "(this sets the environment variable " << env_var_name
+                      << " to a directory including " << plugin_name << ".xml)" << std::endl;
             status.status = PluginStatus<T>::parse_failed;
             return status;
         }
@@ -128,11 +125,9 @@ class PluginManager {
                 auto tags = str2container<std::set<std::string>>(it_tags->second, ", ");
                 // If the plugin_tags contains any tag in tags, this plugin
                 // should be loaded
-                valid_tag = std::any_of(tags.begin(), tags.end(),
-                                        [&](const std::string &tag) {
-                                            return (plugin_tags.find(tag) !=
-                                                    plugin_tags.end());
-                                        });
+                valid_tag = std::any_of(tags.begin(), tags.end(), [&](const std::string& tag) {
+                    return (plugin_tags.find(tag) != plugin_tags.end());
+                });
             }
             if (not valid_tag) {
                 status.status = PluginStatus<T>::invalid_tag;
@@ -165,9 +160,8 @@ class PluginManager {
 
         // Warn the user if multiple plugin libraries were found
         if (matching_plugins.size() > 1) {
-            std::cout << "WARNING: Multiple paths found for plugin:"
-                      << plugin_name_so << std::endl;
-            for (const std::string &plugin_path : matching_plugins) {
+            std::cout << "WARNING: Multiple paths found for plugin:" << plugin_name_so << std::endl;
+            for (const std::string& plugin_path : matching_plugins) {
                 std::cout << plugin_path << std::endl;
             }
         }
@@ -184,8 +178,8 @@ class PluginManager {
             return status;
         }
 
-        std::cout << "ERROR: Could not locate " << plugin_type << "::"
-                  << plugin_name_so << std::endl;
+        std::cout << "ERROR: Could not locate " << plugin_type << "::" << plugin_name_so
+                  << std::endl;
         status.status = PluginStatus<T>::missing;
         return status;
     }
@@ -206,13 +200,13 @@ class PluginManager {
     bool files_checked_ = false;
 
     int check_library(std::string lib_path);
-    PluginPtr make_plugin_helper(std::string &plugin_type, std::string &plugin_name);
+    PluginPtr make_plugin_helper(std::string& plugin_type, std::string& plugin_name);
     bool reload_;
 
     // std::list<PluginPtr> plugins_;
 };
 
 using PluginManagerPtr = std::shared_ptr<PluginManager>;
-} // namespace scrimmage
+}  // namespace scrimmage
 
-#endif // INCLUDE_SCRIMMAGE_PLUGIN_MANAGER_PLUGINMANAGER_H_
+#endif  // INCLUDE_SCRIMMAGE_PLUGIN_MANAGER_PLUGINMANAGER_H_
diff --git a/include/scrimmage/plugin_manager/RegisterPlugin.h b/include/scrimmage/plugin_manager/RegisterPlugin.h
index feaa69421f..987b83d1f3 100644
--- a/include/scrimmage/plugin_manager/RegisterPlugin.h
+++ b/include/scrimmage/plugin_manager/RegisterPlugin.h
@@ -34,31 +34,30 @@
 
 #include <memory>
 
-#define REGISTER_PLUGIN_HELPER(BaseClass, ChildClass, PluginName)   \
-    extern "C" {                                                    \
-        std::shared_ptr<BaseClass> maker() {                        \
-            return std::make_shared<ChildClass>();                  \
-        }                                                           \
-        const char * plugin_name() {                                \
-            return #PluginName;                                     \
-        }                                                           \
-        const char * plugin_type() {                                \
-            return #BaseClass;                                      \
-        }                                                           \
-    }                                                               \
+#define REGISTER_PLUGIN_HELPER(BaseClass, ChildClass, PluginName) \
+    extern "C" {                                                  \
+    std::shared_ptr<BaseClass> maker() {                          \
+        return std::make_shared<ChildClass>();                    \
+    }                                                             \
+    const char* plugin_name() {                                   \
+        return #PluginName;                                       \
+    }                                                             \
+    const char* plugin_type() {                                   \
+        return #BaseClass;                                        \
+    }                                                             \
+    }
 
 #ifdef __clang__
 // http://clang.llvm.org/docs/UsersManual.html#diagnostics
 // https://stackoverflow.com/a/28907242
-#define REGISTER_PLUGIN(BaseClass, ChildClass, PluginName)          \
-    _Pragma("clang diagnostic push")                                \
-    _Pragma("clang diagnostic ignored \"-Wreturn-type-c-linkage\"") \
-    REGISTER_PLUGIN_HELPER(BaseClass, ChildClass, PluginName)       \
-    _Pragma("clang diagnostic pop")
+#define REGISTER_PLUGIN(BaseClass, ChildClass, PluginName)              \
+    _Pragma("clang diagnostic push")                                    \
+        _Pragma("clang diagnostic ignored \"-Wreturn-type-c-linkage\"") \
+            REGISTER_PLUGIN_HELPER(BaseClass, ChildClass, PluginName)   \
+                _Pragma("clang diagnostic pop")
 #else
-#define REGISTER_PLUGIN(BaseClass, ChildClass, PluginName)          \
+#define REGISTER_PLUGIN(BaseClass, ChildClass, PluginName) \
     REGISTER_PLUGIN_HELPER(BaseClass, ChildClass, PluginName)
 #endif
 
-#endif // INCLUDE_SCRIMMAGE_PLUGIN_MANAGER_REGISTERPLUGIN_H_
-
+#endif  // INCLUDE_SCRIMMAGE_PLUGIN_MANAGER_REGISTERPLUGIN_H_
diff --git a/include/scrimmage/plugins/autonomy/APITester/APITester.h b/include/scrimmage/plugins/autonomy/APITester/APITester.h
index 79a8b707bf..8a33a1111a 100644
--- a/include/scrimmage/plugins/autonomy/APITester/APITester.h
+++ b/include/scrimmage/plugins/autonomy/APITester/APITester.h
@@ -35,14 +35,14 @@
 #include <scrimmage/autonomy/Autonomy.h>
 #include <scrimmage/common/CSV.h>
 
-#include <string>
 #include <map>
+#include <string>
 
 namespace scrimmage {
 namespace autonomy {
 class APITester : public scrimmage::Autonomy {
  public:
-    void init(std::map<std::string, std::string> &params) override;
+    void init(std::map<std::string, std::string>& params) override;
     bool step_autonomy(double t, double dt) override;
 
  protected:
@@ -59,6 +59,6 @@ class APITester : public scrimmage::Autonomy {
 
     void write_my_test_values();
 };
-} // namespace autonomy
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_APITESTER_APITESTER_H_
+}  // namespace autonomy
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_APITESTER_APITESTER_H_
diff --git a/include/scrimmage/plugins/autonomy/ArduPilot/ArduPilot.h b/include/scrimmage/plugins/autonomy/ArduPilot/ArduPilot.h
index b763bca9ab..58311ab06a 100644
--- a/include/scrimmage/plugins/autonomy/ArduPilot/ArduPilot.h
+++ b/include/scrimmage/plugins/autonomy/ArduPilot/ArduPilot.h
@@ -35,20 +35,18 @@
 
 #include <scrimmage/autonomy/Autonomy.h>
 #include <scrimmage/math/Angles.h>
-
-#include <scrimmage/plugins/motion/RigidBody6DOF/RigidBody6DOFState.h>
 #include <scrimmage/plugins/controller/JoystickController/AxisScale.h>
+#include <scrimmage/plugins/motion/RigidBody6DOF/RigidBody6DOFState.h>
 
-#include <thread> // NOLINT
-#include <mutex> // NOLINT
-#include <map>
-#include <string>
-#include <list>
-#include <memory>
-
+#include <boost/array.hpp>
 #include <boost/asio.hpp>
 #include <boost/system/error_code.hpp>
-#include <boost/array.hpp>
+#include <list>
+#include <map>
+#include <memory>
+#include <mutex>  // NOLINT
+#include <string>
+#include <thread>  // NOLINT
 
 namespace scrimmage {
 
@@ -67,7 +65,7 @@ class ArduPilot : public scrimmage::Autonomy {
 
     // State packet sent from Scrimmage to ArduPilot
     struct fdm_packet {
-        uint64_t timestamp_us; // simulation time in microseconds
+        uint64_t timestamp_us;  // simulation time in microseconds
         double latitude, longitude;
         double altitude;
         double heading;
@@ -80,7 +78,7 @@ class ArduPilot : public scrimmage::Autonomy {
 
  public:
     ArduPilot();
-    void init(std::map<std::string, std::string> &params) override;
+    void init(std::map<std::string, std::string>& params) override;
     bool step_autonomy(double t, double dt) override;
     void close(double t) override;
 
@@ -100,8 +98,7 @@ class ArduPilot : public scrimmage::Autonomy {
 
     scrimmage::Angles angles_to_gps_;
 
-    fdm_packet state6dof_to_fdm_packet(double t,
-                                       scrimmage::motion::RigidBody6DOFState &state);
+    fdm_packet state6dof_to_fdm_packet(double t, scrimmage::motion::RigidBody6DOFState& state);
 
     boost::asio::io_service recv_io_service_;
     std::shared_ptr<boost::asio::ip::udp::socket> recv_socket_;
@@ -109,13 +106,11 @@ class ArduPilot : public scrimmage::Autonomy {
     boost::array<unsigned char, 100> recv_buffer_;
 
     void start_receive();
-    void parse_receive(const boost::system::error_code& error,
-                        std::size_t num_bytes);
-    void handle_receive(const boost::system::error_code& error,
-                        std::size_t num_bytes);
+    void parse_receive(const boost::system::error_code& error, std::size_t num_bytes);
+    void handle_receive(const boost::system::error_code& error, std::size_t num_bytes);
 
     std::shared_ptr<motion::RigidBody6DOFState> state_6dof_;
 };
-} // namespace autonomy
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_ARDUPILOT_ARDUPILOT_H_
+}  // namespace autonomy
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_ARDUPILOT_ARDUPILOT_H_
diff --git a/include/scrimmage/plugins/autonomy/ArduPilot/PwmState.h b/include/scrimmage/plugins/autonomy/ArduPilot/PwmState.h
index 3c33875444..0ca58df0d2 100644
--- a/include/scrimmage/plugins/autonomy/ArduPilot/PwmState.h
+++ b/include/scrimmage/plugins/autonomy/ArduPilot/PwmState.h
@@ -36,7 +36,6 @@
 #include <scrimmage/math/State.h>
 
 #include <Eigen/Dense>
-
 #include <memory>
 
 namespace scrimmage {
@@ -44,14 +43,26 @@ namespace motion {
 
 class PwmState : public scrimmage::State {
  public:
-    void set_pwm_input(Eigen::VectorXd pwm_input) { pwm_input_ = pwm_input; }
+    void set_pwm_input(Eigen::VectorXd pwm_input) {
+        pwm_input_ = pwm_input;
+    }
 
-    Eigen::VectorXd &pwm_input() { return pwm_input_; }
+    Eigen::VectorXd& pwm_input() {
+        return pwm_input_;
+    }
 
-    void set_pwm_min(double pwm_min) { pwm_min_ = pwm_min; }
-    void set_pwm_max(double pwm_max) { pwm_max_ = pwm_max; }
-    double &pwm_min() { return pwm_min_; }
-    double &pwm_max() { return pwm_max_; }
+    void set_pwm_min(double pwm_min) {
+        pwm_min_ = pwm_min;
+    }
+    void set_pwm_max(double pwm_max) {
+        pwm_max_ = pwm_max;
+    }
+    double& pwm_min() {
+        return pwm_min_;
+    }
+    double& pwm_max() {
+        return pwm_max_;
+    }
 
  protected:
     Eigen::VectorXd pwm_input_;
diff --git a/include/scrimmage/plugins/autonomy/AuctionAssign/AuctionAssign.h b/include/scrimmage/plugins/autonomy/AuctionAssign/AuctionAssign.h
index f795ffe5b8..3a7e077b7f 100644
--- a/include/scrimmage/plugins/autonomy/AuctionAssign/AuctionAssign.h
+++ b/include/scrimmage/plugins/autonomy/AuctionAssign/AuctionAssign.h
@@ -35,15 +35,15 @@
 
 #include <scrimmage/autonomy/Autonomy.h>
 
+#include <limits>
 #include <map>
 #include <string>
-#include <limits>
 
 namespace scrimmage {
 namespace autonomy {
 class AuctionAssign : public scrimmage::Autonomy {
  public:
-    void init(std::map<std::string, std::string> &params) override;
+    void init(std::map<std::string, std::string>& params) override;
     bool step_autonomy(double t, double dt) override;
 
  protected:
@@ -65,6 +65,6 @@ class AuctionAssign : public scrimmage::Autonomy {
     int output_vel_y_idx_ = 0;
     int output_vel_z_idx_ = 0;
 };
-} // namespace autonomy
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_AUCTIONASSIGN_AUCTIONASSIGN_H_
+}  // namespace autonomy
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_AUCTIONASSIGN_AUCTIONASSIGN_H_
diff --git a/include/scrimmage/plugins/autonomy/AutonomyExecutor/AutonomyExecutor.h b/include/scrimmage/plugins/autonomy/AutonomyExecutor/AutonomyExecutor.h
index 220cbcafd7..24671fc7a6 100644
--- a/include/scrimmage/plugins/autonomy/AutonomyExecutor/AutonomyExecutor.h
+++ b/include/scrimmage/plugins/autonomy/AutonomyExecutor/AutonomyExecutor.h
@@ -35,9 +35,9 @@
 #include <scrimmage/autonomy/Autonomy.h>
 #include <scrimmage/parse/ConfigParse.h>
 
-#include <string>
-#include <map>
 #include <list>
+#include <map>
+#include <string>
 
 namespace sc = scrimmage;
 
@@ -45,7 +45,7 @@ namespace scrimmage {
 namespace autonomy {
 class AutonomyExecutor : public scrimmage::Autonomy {
  public:
-    void init(std::map<std::string, std::string> &params) override;
+    void init(std::map<std::string, std::string>& params) override;
     bool step_autonomy(double t, double dt) override;
     bool call_init(std::string autonomy_name, sc::AutonomyPtr autonomy_s);
 
@@ -64,6 +64,6 @@ class AutonomyExecutor : public scrimmage::Autonomy {
     // Value: Input variable index
     std::map<int, int> io_map_;
 };
-} // namespace autonomy
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_AUTONOMYEXECUTOR_AUTONOMYEXECUTOR_H_
+}  // namespace autonomy
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_AUTONOMYEXECUTOR_AUTONOMYEXECUTOR_H_
diff --git a/include/scrimmage/plugins/autonomy/AvoidEntityMS/AvoidEntityMS.h b/include/scrimmage/plugins/autonomy/AvoidEntityMS/AvoidEntityMS.h
index 03cfac4772..b1a6d22559 100644
--- a/include/scrimmage/plugins/autonomy/AvoidEntityMS/AvoidEntityMS.h
+++ b/include/scrimmage/plugins/autonomy/AvoidEntityMS/AvoidEntityMS.h
@@ -35,10 +35,10 @@
 
 #include <scrimmage/plugins/autonomy/MotorSchemas/BehaviorBase.h>
 
-#include <string>
 #include <map>
-#include <vector>
 #include <memory>
+#include <string>
+#include <vector>
 
 namespace scrimmage {
 namespace autonomy {
@@ -46,7 +46,7 @@ namespace motor_schemas {
 class AvoidEntityMS : public scrimmage::autonomy::motor_schemas::BehaviorBase {
  public:
     AvoidEntityMS();
-    void init(std::map<std::string, std::string> &params) override;
+    void init(std::map<std::string, std::string>& params) override;
     bool step_autonomy(double t, double dt) override;
 
  protected:
@@ -59,10 +59,9 @@ class AvoidEntityMS : public scrimmage::autonomy::motor_schemas::BehaviorBase {
 
     bool show_shapes_ = false;
 
-    void avoidance_vectors(ContactMap &contacts,
-                           std::vector<Eigen::Vector3d> &O_vecs);
+    void avoidance_vectors(ContactMap& contacts, std::vector<Eigen::Vector3d>& O_vecs);
 };
-} // namespace motor_schemas
-} // namespace autonomy
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_AVOIDENTITYMS_AVOIDENTITYMS_H_
+}  // namespace motor_schemas
+}  // namespace autonomy
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_AVOIDENTITYMS_AVOIDENTITYMS_H_
diff --git a/include/scrimmage/plugins/autonomy/AvoidWalls/AvoidWalls.h b/include/scrimmage/plugins/autonomy/AvoidWalls/AvoidWalls.h
index dae2f97b45..06393594e2 100644
--- a/include/scrimmage/plugins/autonomy/AvoidWalls/AvoidWalls.h
+++ b/include/scrimmage/plugins/autonomy/AvoidWalls/AvoidWalls.h
@@ -34,7 +34,6 @@
 #define INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_AVOIDWALLS_AVOIDWALLS_H_
 
 #include <scrimmage/autonomy/Autonomy.h>
-
 #include <scrimmage/plugins/sensor/RayTrace/RayTrace.h>
 
 #include <map>
@@ -44,7 +43,7 @@ namespace scrimmage {
 namespace autonomy {
 class AvoidWalls : public scrimmage::Autonomy {
  public:
-    void init(std::map<std::string, std::string> &params) override;
+    void init(std::map<std::string, std::string>& params) override;
     bool step_autonomy(double t, double dt) override;
 
  protected:
@@ -54,6 +53,6 @@ class AvoidWalls : public scrimmage::Autonomy {
     uint8_t heading_idx_ = 0;
     uint8_t speed_idx_ = 0;
 };
-} // namespace autonomy
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_AVOIDWALLS_AVOIDWALLS_H_
+}  // namespace autonomy
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_AVOIDWALLS_AVOIDWALLS_H_
diff --git a/include/scrimmage/plugins/autonomy/Boids/Boids.h b/include/scrimmage/plugins/autonomy/Boids/Boids.h
index 582a8bedd5..cead35f03b 100644
--- a/include/scrimmage/plugins/autonomy/Boids/Boids.h
+++ b/include/scrimmage/plugins/autonomy/Boids/Boids.h
@@ -42,11 +42,11 @@ namespace scrimmage {
 namespace autonomy {
 class Boids : public scrimmage::Autonomy {
  public:
-    void init(std::map<std::string, std::string> &params) override;
+    void init(std::map<std::string, std::string>& params) override;
     bool step_autonomy(double t, double dt) override;
 
  protected:
-    void velocity_controller(Eigen::Vector3d &v);
+    void velocity_controller(Eigen::Vector3d& v);
 
     bool show_shapes_;
     double max_speed_;
diff --git a/include/scrimmage/plugins/autonomy/BoundaryDefense/BoundaryDefense.h b/include/scrimmage/plugins/autonomy/BoundaryDefense/BoundaryDefense.h
index e7a18f41bf..8d9e765526 100644
--- a/include/scrimmage/plugins/autonomy/BoundaryDefense/BoundaryDefense.h
+++ b/include/scrimmage/plugins/autonomy/BoundaryDefense/BoundaryDefense.h
@@ -36,28 +36,28 @@
 #include <scrimmage/plugins/interaction/Boundary/BoundaryBase.h>
 #include <scrimmage/proto/Shape.pb.h>
 
-#include <string>
 #include <map>
-#include <utility>
 #include <memory>
+#include <string>
+#include <utility>
 
 namespace scrimmage {
 namespace autonomy {
 class BoundaryDefense : public scrimmage::Autonomy {
  public:
     BoundaryDefense();
-    void init(std::map<std::string, std::string> &params) override;
+    void init(std::map<std::string, std::string>& params) override;
     bool step_autonomy(double t, double dt) override;
 
  protected:
     int boundary_id_ = -1;
-    std::map<int, std::pair<scrimmage_proto::Shape,
-        std::shared_ptr<interaction::BoundaryBase>>> boundaries_;
+    std::map<int, std::pair<scrimmage_proto::Shape, std::shared_ptr<interaction::BoundaryBase>>>
+        boundaries_;
 
     int output_vel_x_idx_ = 0;
     int output_vel_y_idx_ = 0;
     int output_vel_z_idx_ = 0;
 };
-} // namespace autonomy
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_BOUNDARYDEFENSE_BOUNDARYDEFENSE_H_
+}  // namespace autonomy
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_BOUNDARYDEFENSE_BOUNDARYDEFENSE_H_
diff --git a/include/scrimmage/plugins/autonomy/CommandStringRelay/CommandStringRelay.h b/include/scrimmage/plugins/autonomy/CommandStringRelay/CommandStringRelay.h
index 334437748b..97b9dcf635 100644
--- a/include/scrimmage/plugins/autonomy/CommandStringRelay/CommandStringRelay.h
+++ b/include/scrimmage/plugins/autonomy/CommandStringRelay/CommandStringRelay.h
@@ -34,20 +34,20 @@
 #define INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_COMMANDSTRINGRELAY_COMMANDSTRINGRELAY_H_
 #include <scrimmage/autonomy/Autonomy.h>
 
-#include <string>
 #include <map>
+#include <string>
 
 namespace scrimmage {
 namespace autonomy {
 class CommandStringRelay : public scrimmage::Autonomy {
  public:
     CommandStringRelay();
-    void init(std::map<std::string, std::string> &params) override;
+    void init(std::map<std::string, std::string>& params) override;
     bool step_autonomy(double t, double dt) override;
 
  protected:
     std::map<std::string, scrimmage::PublisherPtr> pubs_;
 };
-} // namespace autonomy
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_COMMANDSTRINGRELAY_COMMANDSTRINGRELAY_H_
+}  // namespace autonomy
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_COMMANDSTRINGRELAY_COMMANDSTRINGRELAY_H_
diff --git a/include/scrimmage/plugins/autonomy/Control3D/Control3D.h b/include/scrimmage/plugins/autonomy/Control3D/Control3D.h
index 0c11b3c244..b5e2c67a99 100644
--- a/include/scrimmage/plugins/autonomy/Control3D/Control3D.h
+++ b/include/scrimmage/plugins/autonomy/Control3D/Control3D.h
@@ -43,9 +43,9 @@ namespace autonomy {
 class Control3D : public scrimmage::Autonomy {
  public:
     Control3D();
-    void init(std::map<std::string, std::string> &params) override;
+    void init(std::map<std::string, std::string>& params) override;
     bool step_autonomy(double t, double dt) override;
 };
-} // namespace autonomy
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_CONTROL3D_CONTROL3D_H_
+}  // namespace autonomy
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_CONTROL3D_CONTROL3D_H_
diff --git a/include/scrimmage/plugins/autonomy/FlightGearMultiplayer/FlightGearMultiplayer.h b/include/scrimmage/plugins/autonomy/FlightGearMultiplayer/FlightGearMultiplayer.h
index 8e51867706..5e491afb8e 100644
--- a/include/scrimmage/plugins/autonomy/FlightGearMultiplayer/FlightGearMultiplayer.h
+++ b/include/scrimmage/plugins/autonomy/FlightGearMultiplayer/FlightGearMultiplayer.h
@@ -33,24 +33,23 @@
 #ifndef INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_FLIGHTGEARMULTIPLAYER_FLIGHTGEARMULTIPLAYER_H_
 #define INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_FLIGHTGEARMULTIPLAYER_FLIGHTGEARMULTIPLAYER_H_
 
+#include <plib/netSocket.h>
 #include <scrimmage/autonomy/Autonomy.h>
 #include <scrimmage/math/Angles.h>
 #include <scrimmage/math/Quaternion.h>
-#include <plib/netSocket.h>
-
-#include <string>
-#include <map>
-#include <memory>
 
 #include <GeographicLib/Geocentric.hpp>
 #include <flightgear/MultiPlayer/mpmessages.hxx>
+#include <map>
+#include <memory>
+#include <string>
 
 namespace scrimmage {
 namespace autonomy {
 class FlightGearMultiplayer : public scrimmage::Autonomy {
  public:
     FlightGearMultiplayer();
-    void init(std::map<std::string, std::string> &params) override;
+    void init(std::map<std::string, std::string>& params) override;
     bool step_autonomy(double t, double dt) override;
 
  protected:
@@ -69,6 +68,6 @@ class FlightGearMultiplayer : public scrimmage::Autonomy {
     struct T_PositionMsg pos_msg_;
     int msg_size_ = 0;
 };
-} // namespace autonomy
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_FLIGHTGEARMULTIPLAYER_FLIGHTGEARMULTIPLAYER_H_
+}  // namespace autonomy
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_FLIGHTGEARMULTIPLAYER_FLIGHTGEARMULTIPLAYER_H_
diff --git a/include/scrimmage/plugins/autonomy/GoToWaypoint/GoToWaypoint.h b/include/scrimmage/plugins/autonomy/GoToWaypoint/GoToWaypoint.h
index 808003b8a5..99477058b0 100644
--- a/include/scrimmage/plugins/autonomy/GoToWaypoint/GoToWaypoint.h
+++ b/include/scrimmage/plugins/autonomy/GoToWaypoint/GoToWaypoint.h
@@ -35,15 +35,15 @@
 #include <scrimmage/autonomy/Autonomy.h>
 #include <scrimmage/plugins/autonomy/WaypointGenerator/WaypointList.h>
 
-#include <vector>
-#include <string>
 #include <map>
+#include <string>
+#include <vector>
 
 namespace scrimmage {
 namespace autonomy {
 class GoToWaypoint : public scrimmage::Autonomy {
  public:
-    void init(std::map<std::string, std::string> &params) override;
+    void init(std::map<std::string, std::string>& params) override;
     bool step_autonomy(double t, double dt) override;
     void publish_waypoint(double t);
 
@@ -53,6 +53,6 @@ class GoToWaypoint : public scrimmage::Autonomy {
     std::vector<std::string> waypoint_;
     scrimmage::PublisherPtr waypoint_list_pub_;
 };
-} // namespace autonomy
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_GOTOWAYPOINT_GOTOWAYPOINT_H_
+}  // namespace autonomy
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_GOTOWAYPOINT_GOTOWAYPOINT_H_
diff --git a/include/scrimmage/plugins/autonomy/GraphvizFSM/GraphvizFSM.h b/include/scrimmage/plugins/autonomy/GraphvizFSM/GraphvizFSM.h
index 42ea4595a9..7c22ccdae4 100644
--- a/include/scrimmage/plugins/autonomy/GraphvizFSM/GraphvizFSM.h
+++ b/include/scrimmage/plugins/autonomy/GraphvizFSM/GraphvizFSM.h
@@ -34,24 +34,23 @@
 #define INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_GRAPHVIZFSM_GRAPHVIZFSM_H_
 #include <scrimmage/autonomy/Autonomy.h>
 
-#include <string>
-#include <map>
-
-#include <boost/lexical_cast.hpp>
-#include <boost/filesystem/path.hpp>
-#include <boost/filesystem/operations.hpp>
 #include <boost/filesystem/exception.hpp>
-#include <boost/graph/graphviz.hpp>
+#include <boost/filesystem/operations.hpp>
+#include <boost/filesystem/path.hpp>
 #include <boost/graph/adjacency_list.hpp>
 #include <boost/graph/graph_utility.hpp>
+#include <boost/graph/graphviz.hpp>
+#include <boost/lexical_cast.hpp>
 #include <boost/property_map/property_map.hpp>
+#include <map>
+#include <string>
 
 namespace scrimmage {
 namespace autonomy {
 class GraphvizFSM : public scrimmage::Autonomy {
  public:
     struct DotVertex {
-        std::string name; //, label, shape;
+        std::string name;  //, label, shape;
     };
 
     struct label_t {
@@ -61,10 +60,12 @@ class GraphvizFSM : public scrimmage::Autonomy {
     typedef boost::property<label_t, std::string> EdgeProperty;
 
     typedef boost::property<boost::graph_name_t, std::string> graph_p;
-    typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS, DotVertex, EdgeProperty, graph_p, boost::listS> graph_t;
+    typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS, DotVertex,
+                                  EdgeProperty, graph_p, boost::listS>
+        graph_t;
 
     GraphvizFSM();
-    void init(std::map<std::string, std::string> &params) override;
+    void init(std::map<std::string, std::string>& params) override;
     bool step_autonomy(double t, double dt) override;
 
  protected:
@@ -74,12 +75,11 @@ class GraphvizFSM : public scrimmage::Autonomy {
 
     boost::property_map<graph_t, label_t>::type label_;
 
-    void update_state_info(
-        boost::graph_traits<graph_t>::vertex_descriptor current_state,
-        boost::graph_traits<graph_t>::vertex_descriptor next_state);
+    void update_state_info(boost::graph_traits<graph_t>::vertex_descriptor current_state,
+                           boost::graph_traits<graph_t>::vertex_descriptor next_state);
 
     bool print_current_state_ = false;
 };
-} // namespace autonomy
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_GRAPHVIZFSM_GRAPHVIZFSM_H_
+}  // namespace autonomy
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_GRAPHVIZFSM_GRAPHVIZFSM_H_
diff --git a/include/scrimmage/plugins/autonomy/JoystickAutonomy/JoystickAutonomy.h b/include/scrimmage/plugins/autonomy/JoystickAutonomy/JoystickAutonomy.h
index 2c00033511..c59fe69d6a 100644
--- a/include/scrimmage/plugins/autonomy/JoystickAutonomy/JoystickAutonomy.h
+++ b/include/scrimmage/plugins/autonomy/JoystickAutonomy/JoystickAutonomy.h
@@ -36,19 +36,19 @@
 #include <scrimmage/autonomy/Autonomy.h>
 #include <scrimmage/plugins/controller/JoystickController/Joystick.h>
 
-#include <string>
 #include <map>
+#include <string>
 
 namespace scrimmage {
 namespace autonomy {
 class JoystickAutonomy : public scrimmage::Autonomy {
  public:
-    void init(std::map<std::string, std::string> &params) override;
+    void init(std::map<std::string, std::string>& params) override;
     bool step_autonomy(double t, double dt) override;
 
  protected:
     scrimmage::controller::Joystick joystick_;
 };
-} // namespace autonomy
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_JOYSTICKAUTONOMY_JOYSTICKAUTONOMY_H_
+}  // namespace autonomy
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_JOYSTICKAUTONOMY_JOYSTICKAUTONOMY_H_
diff --git a/include/scrimmage/plugins/autonomy/MOOSAutonomy/MOOSAutonomy.h b/include/scrimmage/plugins/autonomy/MOOSAutonomy/MOOSAutonomy.h
index 4536f51295..e3837746a2 100644
--- a/include/scrimmage/plugins/autonomy/MOOSAutonomy/MOOSAutonomy.h
+++ b/include/scrimmage/plugins/autonomy/MOOSAutonomy/MOOSAutonomy.h
@@ -36,9 +36,9 @@
 #include <scrimmage/autonomy/Autonomy.h>
 #include <scrimmage/math/Angles.h>
 
-#include <thread> // NOLINT
-#include <string>
 #include <map>
+#include <string>
+#include <thread>  // NOLINT
 
 #include "MOOSNode.h"
 
@@ -47,7 +47,7 @@ namespace autonomy {
 class MOOSAutonomy : public scrimmage::Autonomy {
  public:
     MOOSAutonomy();
-    void init(std::map<std::string, std::string> &params) override;
+    void init(std::map<std::string, std::string>& params) override;
     bool ready() override;
     bool step_autonomy(double t, double dt) override;
 
@@ -69,6 +69,6 @@ class MOOSAutonomy : public scrimmage::Autonomy {
     int desired_alt_idx_ = 0;
     int desired_speed_idx_ = 0;
 };
-} // namespace autonomy
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_MOOSAUTONOMY_MOOSAUTONOMY_H_
+}  // namespace autonomy
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_MOOSAUTONOMY_MOOSAUTONOMY_H_
diff --git a/include/scrimmage/plugins/autonomy/MOOSAutonomy/MOOSNode.h b/include/scrimmage/plugins/autonomy/MOOSAutonomy/MOOSNode.h
index 221dc1a91d..815bb3543d 100644
--- a/include/scrimmage/plugins/autonomy/MOOSAutonomy/MOOSNode.h
+++ b/include/scrimmage/plugins/autonomy/MOOSAutonomy/MOOSNode.h
@@ -37,61 +37,55 @@
 #include <scrimmage/math/State.h>
 #include <string.h>
 
+#include <mutex>  // NOLINT
 #include <string>
-#include <mutex> // NOLINT
 
 namespace scrimmage {
 namespace autonomy {
 class MOOSNode : public CMOOSApp {
  public:
-     // standard construction and destruction
-	MOOSNode();
-	virtual ~MOOSNode();
+    // standard construction and destruction
+    MOOSNode();
+    virtual ~MOOSNode();
 
     bool ready();
 
     scrimmage::State desired_state();
 
-    typedef enum NodeReportType {
-        OWNSHIP = 0,
-        TRUTH_CONTACT,
-        SENSOR_CONTACT
-    } NodeReportType_t;
+    typedef enum NodeReportType { OWNSHIP = 0, TRUTH_CONTACT, SENSOR_CONTACT } NodeReportType_t;
 
     void set_time_warp(double warp);
 
-    bool PublishNodeReport(NodeReportType_t report_type, std::string id,
-                           std::string sensor_id,
-                           double nav_x, double nav_y, double speed,
-                           double heading, double depth, std::string type,
-                           std::string mode, double time,
+    bool PublishNodeReport(NodeReportType_t report_type, std::string id, std::string sensor_id,
+                           double nav_x, double nav_y, double speed, double heading, double depth,
+                           std::string type, std::string mode, double time,
                            std::string frame_number);
 
  protected:
-	double appTick_;
-	double commsTick_;
+    double appTick_;
+    double commsTick_;
     double time_warp_;
 
     bool deployed_;
     std::mutex deployed_mutex_;
 
-	// where we handle new mail
-	bool OnNewMail(MOOSMSG_LIST &NewMail);
+    // where we handle new mail
+    bool OnNewMail(MOOSMSG_LIST& NewMail);
 
-	// where we do the work
-	bool Iterate();
+    // where we do the work
+    bool Iterate();
 
-	// called when we connect to the server
-	bool OnConnectToServer();
+    // called when we connect to the server
+    bool OnConnectToServer();
 
-	// called when we are starting up..
-	bool OnStartUp();
+    // called when we are starting up..
+    bool OnStartUp();
 
     void DoRegistrations();
 
     std::mutex desired_mutex_;
     scrimmage::State desired_;
 };
-} // namespace autonomy
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_MOOSAUTONOMY_MOOSNODE_H_
+}  // namespace autonomy
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_MOOSAUTONOMY_MOOSNODE_H_
diff --git a/include/scrimmage/plugins/autonomy/MotorSchemas/BehaviorBase.h b/include/scrimmage/plugins/autonomy/MotorSchemas/BehaviorBase.h
index 85adc39d87..cb4c67abd8 100644
--- a/include/scrimmage/plugins/autonomy/MotorSchemas/BehaviorBase.h
+++ b/include/scrimmage/plugins/autonomy/MotorSchemas/BehaviorBase.h
@@ -46,11 +46,11 @@ namespace motor_schemas {
 class BehaviorBase : public scrimmage::Autonomy {
  public:
     BehaviorBase();
-    Eigen::Vector3d &desired_vector();
-    void set_gain(const double &gain);
-    const double &gain();
-    void set_max_vector_length(const double &max_vector_length);
-    void configure_contacts(std::map<std::string, std::string> &params);
+    Eigen::Vector3d& desired_vector();
+    void set_gain(const double& gain);
+    const double& gain();
+    void set_max_vector_length(const double& max_vector_length);
+    void configure_contacts(std::map<std::string, std::string>& params);
 
  protected:
     Eigen::Vector3d desired_vector_;
diff --git a/include/scrimmage/plugins/autonomy/MotorSchemas/MotorSchemas.h b/include/scrimmage/plugins/autonomy/MotorSchemas/MotorSchemas.h
index a0b96bbf11..4a06202bfb 100644
--- a/include/scrimmage/plugins/autonomy/MotorSchemas/MotorSchemas.h
+++ b/include/scrimmage/plugins/autonomy/MotorSchemas/MotorSchemas.h
@@ -33,20 +33,19 @@
 #ifndef INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_MOTORSCHEMAS_MOTORSCHEMAS_H_
 #define INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_MOTORSCHEMAS_MOTORSCHEMAS_H_
 
-#include <scrimmage/plugins/autonomy/MotorSchemas/BehaviorBase.h>
-
 #include <scrimmage/autonomy/Autonomy.h>
+#include <scrimmage/plugins/autonomy/MotorSchemas/BehaviorBase.h>
 
-#include <map>
-#include <string>
 #include <list>
+#include <map>
 #include <memory>
+#include <string>
 
 namespace scrimmage {
 namespace autonomy {
 class MotorSchemas : public scrimmage::Autonomy {
  public:
-    void init(std::map<std::string, std::string> &params) override;
+    void init(std::map<std::string, std::string>& params) override;
     bool step_autonomy(double t, double dt) override;
 
  protected:
@@ -73,6 +72,6 @@ class MotorSchemas : public scrimmage::Autonomy {
 
     scrimmage_proto::ShapePtr line_shape_;
 };
-} // namespace autonomy
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_MOTORSCHEMAS_MOTORSCHEMAS_H_
+}  // namespace autonomy
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_MOTORSCHEMAS_MOTORSCHEMAS_H_
diff --git a/include/scrimmage/plugins/autonomy/MoveToGoalMS/MoveToGoalMS.h b/include/scrimmage/plugins/autonomy/MoveToGoalMS/MoveToGoalMS.h
index d1c15d596b..95867c45cf 100644
--- a/include/scrimmage/plugins/autonomy/MoveToGoalMS/MoveToGoalMS.h
+++ b/include/scrimmage/plugins/autonomy/MoveToGoalMS/MoveToGoalMS.h
@@ -47,7 +47,7 @@ namespace motor_schemas {
 class MoveToGoalMS : public scrimmage::autonomy::motor_schemas::BehaviorBase {
  public:
     MoveToGoalMS();
-    void init(std::map<std::string, std::string> &params) override;
+    void init(std::map<std::string, std::string>& params) override;
     bool step_autonomy(double t, double dt) override;
 
  protected:
diff --git a/include/scrimmage/plugins/autonomy/Predator/Predator.h b/include/scrimmage/plugins/autonomy/Predator/Predator.h
index aa5e8d4403..97c1503389 100644
--- a/include/scrimmage/plugins/autonomy/Predator/Predator.h
+++ b/include/scrimmage/plugins/autonomy/Predator/Predator.h
@@ -42,7 +42,7 @@ namespace scrimmage {
 namespace autonomy {
 class Predator : public scrimmage::Autonomy {
  public:
-    void init(std::map<std::string, std::string> &params) override;
+    void init(std::map<std::string, std::string>& params) override;
     bool step_autonomy(double t, double dt) override;
 
  protected:
@@ -60,6 +60,6 @@ class Predator : public scrimmage::Autonomy {
     int desired_heading_idx_ = 0;
     int desired_speed_idx_ = 0;
 };
-} // namespace autonomy
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_PREDATOR_PREDATOR_H_
+}  // namespace autonomy
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_PREDATOR_PREDATOR_H_
diff --git a/include/scrimmage/plugins/autonomy/PyAutonomy/PyAutonomy.h b/include/scrimmage/plugins/autonomy/PyAutonomy/PyAutonomy.h
index 7d54314fa5..78b36c2974 100644
--- a/include/scrimmage/plugins/autonomy/PyAutonomy/PyAutonomy.h
+++ b/include/scrimmage/plugins/autonomy/PyAutonomy/PyAutonomy.h
@@ -35,54 +35,54 @@
 #include <scrimmage/autonomy/Autonomy.h>
 #include <scrimmage/entity/Contact.h>
 
-#include <string>
 #include <map>
 #include <memory>
+#include <string>
 
 namespace scrimmage {
 namespace autonomy {
 class PyAutonomy : public scrimmage::Autonomy {
  public:
     PyAutonomy();
-    void init(std::map<std::string, std::string> &params) override;
+    void init(std::map<std::string, std::string>& params) override;
     bool step_autonomy(double t, double dt) override;
 
-    std::string type() override {return std::string("PyAutonomy");}
+    std::string type() override {
+        return std::string("PyAutonomy");
+    }
 
-    void set_contacts(scrimmage::ContactMapPtr &contacts) override {
+    void set_contacts(scrimmage::ContactMapPtr& contacts) override {
         contacts_ = contacts;
         py_contacts_.clear();
-        for (auto &kv : *contacts) {
-            py_contacts_[pybind11::int_(kv.second.id().id())] =
-                contact2py(kv.second);
+        for (auto& kv : *contacts) {
+            py_contacts_[pybind11::int_(kv.second.id().id())] = contact2py(kv.second);
         }
     }
 
-    void set_contacts_from_plugin(scrimmage::AutonomyPtr &ptr) override {
-        std::shared_ptr<PyAutonomy> py_ptr =
-            std::static_pointer_cast<PyAutonomy>(ptr);
+    void set_contacts_from_plugin(scrimmage::AutonomyPtr& ptr) override {
+        std::shared_ptr<PyAutonomy> py_ptr = std::static_pointer_cast<PyAutonomy>(ptr);
         contacts_ = py_ptr->contacts_;
         py_contacts_ = py_ptr->py_contacts_;
     }
 
-    void set_state(scrimmage::StatePtr &state) override {
+    void set_state(scrimmage::StatePtr& state) override {
         state_ = state;
     }
 
  protected:
     void cache_python_vars();
-    pybind11::object get_py_obj(std::map<std::string, std::string> &params);
-    void init_py_obj(std::map<std::string, std::string> &params);
+    pybind11::object get_py_obj(std::map<std::string, std::string>& params);
+    void init_py_obj(std::map<std::string, std::string>& params);
 
     // void sub_msgs_to_py_subs();
     // void py_pub_msgs_to_pubs();
     // void sync_topics();
 
-    void print_py_traceback(pybind11::error_already_set &e);
+    void print_py_traceback(pybind11::error_already_set& e);
 
     bool step_autonomy_called_ = false;
 
-    pybind11::object state2py(scrimmage::StatePtr &state);
+    pybind11::object state2py(scrimmage::StatePtr& state);
     pybind11::object contact2py(scrimmage::Contact contact);
     static std::shared_ptr<scrimmage_proto::Shape> py2shape(const pybind11::handle& py_handle);
 
@@ -101,6 +101,6 @@ class PyAutonomy : public scrimmage::Autonomy {
 
     std::map<scrimmage::Contact::Type, pybind11::object> py_contact_types_;
 };
-} // namespace autonomy
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_PYAUTONOMY_PYAUTONOMY_H_
+}  // namespace autonomy
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_PYAUTONOMY_PYAUTONOMY_H_
diff --git a/include/scrimmage/plugins/autonomy/RLConsensus/RLConsensus.h b/include/scrimmage/plugins/autonomy/RLConsensus/RLConsensus.h
index cc310827a1..6e2e0964dd 100644
--- a/include/scrimmage/plugins/autonomy/RLConsensus/RLConsensus.h
+++ b/include/scrimmage/plugins/autonomy/RLConsensus/RLConsensus.h
@@ -45,7 +45,7 @@ class RLConsensus : public scrimmage::autonomy::RLSimple {
     void set_environment() override;
     std::tuple<bool, double, pybind11::dict> calc_reward() override;
 };
-} // namespace autonomy
-} // namespace scrimmage
+}  // namespace autonomy
+}  // namespace scrimmage
 
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_RLCONSENSUS_RLCONSENSUS_H_
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_RLCONSENSUS_RLCONSENSUS_H_
diff --git a/include/scrimmage/plugins/autonomy/RLSimple/RLSimple.h b/include/scrimmage/plugins/autonomy/RLSimple/RLSimple.h
index 23bcc95013..65c9e78d52 100644
--- a/include/scrimmage/plugins/autonomy/RLSimple/RLSimple.h
+++ b/include/scrimmage/plugins/autonomy/RLSimple/RLSimple.h
@@ -44,7 +44,7 @@ namespace autonomy {
 
 class RLSimple : public ScrimmageOpenAIAutonomy {
  public:
-    void init_helper(std::map<std::string, std::string> &params) override;
+    void init_helper(std::map<std::string, std::string>& params) override;
     bool step_helper() override;
 
     void set_environment() override;
@@ -61,9 +61,9 @@ class RLSimple : public ScrimmageOpenAIAutonomy {
     uint8_t output_vel_x_idx_ = 0;
     uint8_t output_vel_y_idx_ = 0;
 
-    double max_speed_ = 0; // for continuous case
+    double max_speed_ = 0;  // for continuous case
 };
-} // namespace autonomy
-} // namespace scrimmage
+}  // namespace autonomy
+}  // namespace scrimmage
 
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_RLSIMPLE_RLSIMPLE_H_
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_RLSIMPLE_RLSIMPLE_H_
diff --git a/include/scrimmage/plugins/autonomy/ROSAirSim/ROSAirSim.h b/include/scrimmage/plugins/autonomy/ROSAirSim/ROSAirSim.h
index dc1acdbca1..0ac05c5481 100644
--- a/include/scrimmage/plugins/autonomy/ROSAirSim/ROSAirSim.h
+++ b/include/scrimmage/plugins/autonomy/ROSAirSim/ROSAirSim.h
@@ -31,32 +31,29 @@
 
 #ifndef INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_ROSAIRSIM_ROSAIRSIM_H_
 #define INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_ROSAIRSIM_ROSAIRSIM_H_
-#include <scrimmage/autonomy/Autonomy.h>
-
-#include <ros/ros.h>
-#include <image_transport/image_transport.h>
 #include <cv_bridge/cv_bridge.h>
-#include <sensor_msgs/PointCloud2.h>
+#include <geometry_msgs/Twist.h>
+#include <image_transport/image_transport.h>
+#include <ros/ros.h>
+#include <scrimmage/autonomy/Autonomy.h>
+#include <scrimmage/plugins/sensor/AirSimSensor/AirSimSensor.h>
 #include <sensor_msgs/CameraInfo.h>
 #include <sensor_msgs/Image.h>
 #include <sensor_msgs/Imu.h>
+#include <sensor_msgs/PointCloud2.h>
+#include <tf2/LinearMath/Quaternion.h>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
-#include <tf2_ros/transform_broadcaster.h>
 #include <tf2_ros/static_transform_broadcaster.h>
+#include <tf2_ros/transform_broadcaster.h>
 
-#include <tf2/LinearMath/Quaternion.h>
-#include <geometry_msgs/Twist.h>
-#include <scrimmage/plugins/sensor/AirSimSensor/AirSimSensor.h>
-
-#include <string>
 #include <map>
 #include <memory>
+#include <string>
 #include <vector>
 
 // Eigen libraries
 #include "Eigen/Core"
 #include "Eigen/Geometry"
-
 #include "common/AirSimSettings.hpp"
 typedef msr::airlib::AirSimSettings::LidarSetting LidarSetting;
 
@@ -66,10 +63,11 @@ namespace autonomy {
 class ROSAirSim : public scrimmage::Autonomy {
  public:
     ROSAirSim();
-    Eigen::Isometry3f get_vehicle_world_pose_from_NED_to_ENU(Eigen::Isometry3f vehicle_pose_world_NED);
-    Eigen::Isometry3f get_sensor_pose_from_worldNED_to_vehicleENU(Eigen::Isometry3f vehicle_position_NED,
-                                                                  Eigen::Isometry3f sensor_position_NED);
-    void init(std::map<std::string, std::string> &params) override;
+    Eigen::Isometry3f get_vehicle_world_pose_from_NED_to_ENU(
+        Eigen::Isometry3f vehicle_pose_world_NED);
+    Eigen::Isometry3f get_sensor_pose_from_worldNED_to_vehicleENU(
+        Eigen::Isometry3f vehicle_position_NED, Eigen::Isometry3f sensor_position_NED);
+    void init(std::map<std::string, std::string>& params) override;
     bool step_autonomy(double t, double dt) override;
 
  protected:
@@ -117,6 +115,6 @@ class ROSAirSim : public scrimmage::Autonomy {
     std::string ros_name_;
     std::string ros_namespace_;
 };
-} // namespace autonomy
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_ROSAIRSIM_ROSAIRSIM_H_
+}  // namespace autonomy
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_ROSAIRSIM_ROSAIRSIM_H_
diff --git a/include/scrimmage/plugins/autonomy/ROSAutonomy/ROSAutonomy.h b/include/scrimmage/plugins/autonomy/ROSAutonomy/ROSAutonomy.h
index 46edc2981e..9f9d7dab4d 100644
--- a/include/scrimmage/plugins/autonomy/ROSAutonomy/ROSAutonomy.h
+++ b/include/scrimmage/plugins/autonomy/ROSAutonomy/ROSAutonomy.h
@@ -33,26 +33,25 @@
 #ifndef INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_ROSAUTONOMY_ROSAUTONOMY_H_
 #define INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_ROSAUTONOMY_ROSAUTONOMY_H_
 
-#include <ros/ros.h>
 #include <geometry_msgs/Twist.h>
 #include <nav_msgs/Odometry.h>
-#include <sensor_msgs/LaserScan.h>
-#include <tf/transform_broadcaster.h>
-
+#include <ros/ros.h>
 #include <scrimmage/autonomy/Autonomy.h>
-#include <scrimmage/pubsub/Subscriber.h>
 #include <scrimmage/plugins/sensor/RayTrace/RayTrace.h>
+#include <scrimmage/pubsub/Subscriber.h>
+#include <sensor_msgs/LaserScan.h>
+#include <tf/transform_broadcaster.h>
 
 #include <map>
-#include <string>
 #include <memory>
+#include <string>
 
 namespace scrimmage {
 namespace autonomy {
 class ROSAutonomy : public scrimmage::Autonomy {
  public:
     ROSAutonomy();
-    void init(std::map<std::string, std::string> &params) override;
+    void init(std::map<std::string, std::string>& params) override;
     bool step_autonomy(double t, double dt) override;
     void cmd_vel_cb(const geometry_msgs::Twist::ConstPtr& msg);
 
@@ -82,6 +81,6 @@ class ROSAutonomy : public scrimmage::Autonomy {
     int pitch_rate_idx_ = 0;
     int velocity_z_idx_ = 0;
 };
-} // namespace autonomy
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_ROSAUTONOMY_ROSAUTONOMY_H_
+}  // namespace autonomy
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_ROSAUTONOMY_ROSAUTONOMY_H_
diff --git a/include/scrimmage/plugins/autonomy/ROSControl/ROSControl.h b/include/scrimmage/plugins/autonomy/ROSControl/ROSControl.h
index 4bfbb4deb0..ddea399b95 100644
--- a/include/scrimmage/plugins/autonomy/ROSControl/ROSControl.h
+++ b/include/scrimmage/plugins/autonomy/ROSControl/ROSControl.h
@@ -33,34 +33,32 @@
 #ifndef INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_ROSCONTROL_ROSCONTROL_H_
 #define INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_ROSCONTROL_ROSCONTROL_H_
 
-#include <ros/ros.h>
 #include <geometry_msgs/Twist.h>
 #include <nav_msgs/Odometry.h>
-#include <sensor_msgs/LaserScan.h>
-#include <tf/transform_broadcaster.h>
-
+#include <ros/ros.h>
 #include <scrimmage/autonomy/Autonomy.h>
 #include <scrimmage/pubsub/Subscriber.h>
+#include <sensor_msgs/LaserScan.h>
+#include <tf/transform_broadcaster.h>
 
-#include <map>
-#include <vector>
-#include <string>
 #include <fstream>
+#include <map>
 #include <memory>
+#include <string>
+#include <vector>
 
 namespace scrimmage {
 namespace autonomy {
 class ROSControl : public scrimmage::Autonomy {
  public:
     ROSControl();
-    void init(std::map<std::string, std::string> &params) override;
+    void init(std::map<std::string, std::string>& params) override;
     bool step_autonomy(double t, double dt) override;
     void cmd_vel_cb(const geometry_msgs::Twist::ConstPtr& msg);
 
  protected:
     void zero_ctrls();
-    void ctrl_filter(const std::vector<double> &x, std::vector<double> &dxdt,
-                     double t);
+    void ctrl_filter(const std::vector<double>& x, std::vector<double>& dxdt, double t);
 
     std::shared_ptr<ros::NodeHandle> nh_;
 
@@ -71,6 +69,6 @@ class ROSControl : public scrimmage::Autonomy {
     std::vector<double> x_;
     // std::ofstream ofs_;
 };
-} // namespace autonomy
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_ROSCONTROL_ROSCONTROL_H_
+}  // namespace autonomy
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_ROSCONTROL_ROSCONTROL_H_
diff --git a/include/scrimmage/plugins/autonomy/ScrimmageOpenAIAutonomy/ActorFunc.h b/include/scrimmage/plugins/autonomy/ScrimmageOpenAIAutonomy/ActorFunc.h
index c33aecb705..5c8e718b02 100644
--- a/include/scrimmage/plugins/autonomy/ScrimmageOpenAIAutonomy/ActorFunc.h
+++ b/include/scrimmage/plugins/autonomy/ScrimmageOpenAIAutonomy/ActorFunc.h
@@ -35,10 +35,10 @@
 #include <scrimmage/common/Visibility.h>
 
 #include <map>
-#include <vector>
+#include <memory>
 #include <string>
 #include <tuple>
-#include <memory>
+#include <vector>
 
 namespace pybind11 {
 class object;
@@ -51,17 +51,14 @@ class ScrimmageOpenAIAutonomy;
 class OpenAIActions;
 class OpenAIObservations;
 
-enum class CombineActors {NO, YES};
-enum class UseGlobalSensor {NO, YES};
-enum class StaticObsSpace {NO, YES};
+enum class CombineActors { NO, YES };
+enum class UseGlobalSensor { NO, YES };
+enum class StaticObsSpace { NO, YES };
 
 std::tuple<OpenAIActions, OpenAIObservations, pybind11::object> DLL_PUBLIC
-init_actor_func(
-    std::vector<std::shared_ptr<ScrimmageOpenAIAutonomy>> autonomies,
-    const std::map<std::string, std::string> &params,
-    CombineActors combine_actors,
-    UseGlobalSensor global_sensor,
-    bool grpc_mode);
-} // namespace autonomy
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_SCRIMMAGEOPENAIAUTONOMY_ACTORFUNC_H_
+init_actor_func(std::vector<std::shared_ptr<ScrimmageOpenAIAutonomy>> autonomies,
+                const std::map<std::string, std::string>& params, CombineActors combine_actors,
+                UseGlobalSensor global_sensor, bool grpc_mode);
+}  // namespace autonomy
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_SCRIMMAGEOPENAIAUTONOMY_ACTORFUNC_H_
diff --git a/include/scrimmage/plugins/autonomy/ScrimmageOpenAIAutonomy/OpenAIActions.h b/include/scrimmage/plugins/autonomy/ScrimmageOpenAIAutonomy/OpenAIActions.h
index f14991a45b..36460b4336 100644
--- a/include/scrimmage/plugins/autonomy/ScrimmageOpenAIAutonomy/OpenAIActions.h
+++ b/include/scrimmage/plugins/autonomy/ScrimmageOpenAIAutonomy/OpenAIActions.h
@@ -34,11 +34,10 @@
 #define INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_SCRIMMAGEOPENAIAUTONOMY_OPENAIACTIONS_H_
 
 #include <pybind11/pybind11.h>
-
 #include <scrimmage/common/Visibility.h>
 
-#include <vector>
 #include <memory>
+#include <vector>
 
 namespace scrimmage {
 namespace autonomy {
@@ -48,7 +47,7 @@ class ScrimmageOpenAIAutonomy;
 class DLL_PUBLIC OpenAIActions {
  public:
     OpenAIActions();
-    std::vector<std::shared_ptr<ScrimmageOpenAIAutonomy>> &ext_ctrl_vec();
+    std::vector<std::shared_ptr<ScrimmageOpenAIAutonomy>>& ext_ctrl_vec();
     void create_action_space(bool combine_actors);
     void distribute_action(pybind11::object action, bool combine_actors);
 
@@ -61,6 +60,6 @@ class DLL_PUBLIC OpenAIActions {
     pybind11::object asarray_;
 };
 
-} // namespace autonomy
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_SCRIMMAGEOPENAIAUTONOMY_OPENAIACTIONS_H_
+}  // namespace autonomy
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_SCRIMMAGEOPENAIAUTONOMY_OPENAIACTIONS_H_
diff --git a/include/scrimmage/plugins/autonomy/ScrimmageOpenAIAutonomy/OpenAIObservations.h b/include/scrimmage/plugins/autonomy/ScrimmageOpenAIAutonomy/OpenAIObservations.h
index fffd422397..e33ab4d5ac 100644
--- a/include/scrimmage/plugins/autonomy/ScrimmageOpenAIAutonomy/OpenAIObservations.h
+++ b/include/scrimmage/plugins/autonomy/ScrimmageOpenAIAutonomy/OpenAIObservations.h
@@ -34,13 +34,12 @@
 #define INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_SCRIMMAGEOPENAIAUTONOMY_OPENAIOBSERVATIONS_H_
 
 #include <pybind11/pybind11.h>
-
 #include <scrimmage/common/Visibility.h>
 
+#include <memory>
 #include <string>
-#include <vector>
 #include <unordered_map>
-#include <memory>
+#include <vector>
 
 namespace scrimmage {
 
@@ -58,26 +57,33 @@ class DLL_PUBLIC OpenAIObservations {
     OpenAIObservations();
 
     void create_observation_space(size_t num_entities, bool static_obs_space);
-    pybind11::object update_observation(size_t num_entities,
-                                        bool static_obs_space);
+    pybind11::object update_observation(size_t num_entities, bool static_obs_space);
 
-    std::vector<std::vector<std::shared_ptr<sensor::ScrimmageOpenAISensor>>>&
-            ext_sensor_vec() {return ext_sensor_vec_;}
+    std::vector<std::vector<std::shared_ptr<sensor::ScrimmageOpenAISensor>>>& ext_sensor_vec() {
+        return ext_sensor_vec_;
+    }
 
-    void add_sensors(const std::unordered_map<std::string, SensorPtr> &sensors);
+    void add_sensors(const std::unordered_map<std::string, SensorPtr>& sensors);
 
     pybind11::object observation_space;
     pybind11::object observation;
 
-    void set_global_sensor(bool global_sensor) {global_sensor_ = global_sensor;}
-    bool get_global_sensor() const {return global_sensor_;}
+    void set_global_sensor(bool global_sensor) {
+        global_sensor_ = global_sensor;
+    }
+    bool get_global_sensor() const {
+        return global_sensor_;
+    }
 
-    void set_combine_actors(bool combine_actors) {combine_actors_ = combine_actors;}
-    bool get_combine_actors() const {return combine_actors_;}
+    void set_combine_actors(bool combine_actors) {
+        combine_actors_ = combine_actors;
+    }
+    bool get_combine_actors() const {
+        return combine_actors_;
+    }
 
  protected:
-    std::vector<std::vector<std::shared_ptr<sensor::ScrimmageOpenAISensor>>>
-            ext_sensor_vec_;
+    std::vector<std::vector<std::shared_ptr<sensor::ScrimmageOpenAISensor>>> ext_sensor_vec_;
 
     pybind11::object tuple_space_;
     pybind11::object box_space_;
@@ -85,7 +91,7 @@ class DLL_PUBLIC OpenAIObservations {
     bool combine_actors_ = false;
     bool global_sensor_ = false;
 };
-} // namespace autonomy
-} // namespace scrimmage
+}  // namespace autonomy
+}  // namespace scrimmage
 
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_SCRIMMAGEOPENAIAUTONOMY_OPENAIOBSERVATIONS_H_
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_SCRIMMAGEOPENAIAUTONOMY_OPENAIOBSERVATIONS_H_
diff --git a/include/scrimmage/plugins/autonomy/ScrimmageOpenAIAutonomy/OpenAIUtils.h b/include/scrimmage/plugins/autonomy/ScrimmageOpenAIAutonomy/OpenAIUtils.h
index 93b93e85e7..3cfba9b6e6 100644
--- a/include/scrimmage/plugins/autonomy/ScrimmageOpenAIAutonomy/OpenAIUtils.h
+++ b/include/scrimmage/plugins/autonomy/ScrimmageOpenAIAutonomy/OpenAIUtils.h
@@ -34,32 +34,28 @@
 #define INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_SCRIMMAGEOPENAIAUTONOMY_OPENAIUTILS_H_
 
 #include <pybind11/pybind11.h>
-
 #include <scrimmage/common/Visibility.h>
 
-#include <utility>
 #include <string>
+#include <utility>
 #include <vector>
 
 namespace scrimmage {
 
 namespace autonomy {
 
-pybind11::object DLL_PUBLIC get_gym_space(const std::string &type);
+pybind11::object DLL_PUBLIC get_gym_space(const std::string& type);
 
-void DLL_PUBLIC to_continuous(
-        std::vector<std::pair<double, double>> &p,
-        pybind11::list &minima,
-        pybind11::list &maxima);
+void DLL_PUBLIC to_continuous(std::vector<std::pair<double, double>>& p, pybind11::list& minima,
+                              pybind11::list& maxima);
 
-void DLL_PUBLIC to_discrete(std::vector<int> &p, pybind11::list &maxima);
+void DLL_PUBLIC to_discrete(std::vector<int>& p, pybind11::list& maxima);
 
-pybind11::object DLL_PUBLIC create_space(
-        pybind11::list discrete_maxima,
-        pybind11::list continuous_minima,
-        pybind11::list continuous_maxima);
+pybind11::object DLL_PUBLIC create_space(pybind11::list discrete_maxima,
+                                         pybind11::list continuous_minima,
+                                         pybind11::list continuous_maxima);
 
-} // namespace autonomy
-} // namespace scrimmage
+}  // namespace autonomy
+}  // namespace scrimmage
 
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_SCRIMMAGEOPENAIAUTONOMY_OPENAIUTILS_H_
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_SCRIMMAGEOPENAIAUTONOMY_OPENAIUTILS_H_
diff --git a/include/scrimmage/plugins/autonomy/ScrimmageOpenAIAutonomy/ScrimmageOpenAIAutonomy.h b/include/scrimmage/plugins/autonomy/ScrimmageOpenAIAutonomy/ScrimmageOpenAIAutonomy.h
index 6efb89dfab..bc7f9617c6 100644
--- a/include/scrimmage/plugins/autonomy/ScrimmageOpenAIAutonomy/ScrimmageOpenAIAutonomy.h
+++ b/include/scrimmage/plugins/autonomy/ScrimmageOpenAIAutonomy/ScrimmageOpenAIAutonomy.h
@@ -33,14 +33,13 @@
 #ifndef INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_SCRIMMAGEOPENAIAUTONOMY_SCRIMMAGEOPENAIAUTONOMY_H_
 #define INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_SCRIMMAGEOPENAIAUTONOMY_SCRIMMAGEOPENAIAUTONOMY_H_
 
+#include <pybind11/pybind11.h>
 #include <scrimmage/autonomy/Autonomy.h>
 #include <scrimmage/common/Visibility.h>
-#include <scrimmage/plugins/autonomy/ScrimmageOpenAIAutonomy/OpenAIObservations.h>
 #include <scrimmage/plugins/autonomy/ScrimmageOpenAIAutonomy/OpenAIActions.h>
+#include <scrimmage/plugins/autonomy/ScrimmageOpenAIAutonomy/OpenAIObservations.h>
 #include <scrimmage/plugins/autonomy/ScrimmageOpenAIAutonomy/OpenAIUtils.h>
 
-#include <pybind11/pybind11.h>
-
 #if ENABLE_GRPC
 #include <grpc++/grpc++.h>
 #include <scrimmage/network/ScrimmageServiceImpl.h>
@@ -48,11 +47,11 @@
 #endif
 
 #include <map>
-#include <vector>
+#include <memory>
 #include <string>
-#include <utility>
 #include <tuple>
-#include <memory>
+#include <utility>
+#include <vector>
 
 namespace scrimmage {
 
@@ -73,26 +72,32 @@ class DLL_PUBLIC ScrimmageOpenAIAutonomy : public scrimmage::Autonomy {
     ScrimmageOpenAIAutonomy();
 
     // normal overrides
-    void init(std::map<std::string, std::string> &params) final;
+    void init(std::map<std::string, std::string>& params) final;
     bool step_autonomy(double t, double dt) final;
 
     // new overrides
-    virtual void init_helper(std::map<std::string, std::string> &/*params*/) {}
-    virtual bool step_helper() {return true;}
-
-    virtual void set_environment() {}
+    virtual void init_helper(std::map<std::string, std::string>& /*params*/) {
+    }
+    virtual bool step_helper() {
+        return true;
+    }
+
+    virtual void set_environment() {
+    }
     std::tuple<bool, double, pybind11::dict> calculate_reward();
     std::pair<double, double> reward_range;
     EnvParams action_space;
     EnvValues action;
-    int self_id() {return self_id_;}
+    int self_id() {
+        return self_id_;
+    }
 
  protected:
 #if ENABLE_GRPC
     std::unique_ptr<scrimmage_proto::OpenAI::Stub> openai_stub_;
     std::unique_ptr<grpc::Server> server_;
-    boost::optional<scrimmage_proto::Action> get_action(scrimmage_proto::Obs &observation);
-    pybind11::object convert_proto_action(const scrimmage_proto::Action &proto_act);
+    boost::optional<scrimmage_proto::Action> get_action(scrimmage_proto::Obs& observation);
+    pybind11::object convert_proto_action(const scrimmage_proto::Action& proto_act);
     scrimmage_proto::Obs obs_to_proto();
     bool send_env();
 
@@ -117,6 +122,6 @@ class DLL_PUBLIC ScrimmageOpenAIAutonomy : public scrimmage::Autonomy {
     virtual std::tuple<bool, double, pybind11::dict> calc_reward();
     bool setup_complete_ = false;
 };
-} // namespace autonomy
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_SCRIMMAGEOPENAIAUTONOMY_SCRIMMAGEOPENAIAUTONOMY_H_
+}  // namespace autonomy
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_SCRIMMAGEOPENAIAUTONOMY_SCRIMMAGEOPENAIAUTONOMY_H_
diff --git a/include/scrimmage/plugins/autonomy/ShapeDraw/ShapeDraw.h b/include/scrimmage/plugins/autonomy/ShapeDraw/ShapeDraw.h
index 39061754bd..2177862615 100644
--- a/include/scrimmage/plugins/autonomy/ShapeDraw/ShapeDraw.h
+++ b/include/scrimmage/plugins/autonomy/ShapeDraw/ShapeDraw.h
@@ -34,15 +34,15 @@
 #define INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_SHAPEDRAW_SHAPEDRAW_H_
 #include <scrimmage/autonomy/Autonomy.h>
 
-#include <string>
 #include <map>
+#include <string>
 
 namespace scrimmage {
 namespace autonomy {
 class ShapeDraw : public scrimmage::Autonomy {
  public:
     ShapeDraw();
-    void init(std::map<std::string, std::string> &params) override;
+    void init(std::map<std::string, std::string>& params) override;
     bool step_autonomy(double t, double dt) override;
 
  protected:
@@ -56,6 +56,6 @@ class ShapeDraw : public scrimmage::Autonomy {
     scrimmage_proto::ShapePtr cuboid_shape_;
     scrimmage_proto::ShapePtr mesh_shape_;
 };
-} // namespace autonomy
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_SHAPEDRAW_SHAPEDRAW_H_
+}  // namespace autonomy
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_SHAPEDRAW_SHAPEDRAW_H_
diff --git a/include/scrimmage/plugins/autonomy/Straight/Straight.h b/include/scrimmage/plugins/autonomy/Straight/Straight.h
index 669642e196..ef88859bbe 100644
--- a/include/scrimmage/plugins/autonomy/Straight/Straight.h
+++ b/include/scrimmage/plugins/autonomy/Straight/Straight.h
@@ -36,7 +36,6 @@
 #include <scrimmage/entity/Contact.h>
 
 #include <Eigen/Dense>
-
 #include <map>
 #include <memory>
 #include <string>
@@ -50,7 +49,7 @@ class BoundaryBase;
 namespace autonomy {
 class Straight : public scrimmage::Autonomy {
  public:
-    void init(std::map<std::string, std::string> &params) override;
+    void init(std::map<std::string, std::string>& params) override;
     bool step_autonomy(double t, double dt) override;
 
  protected:
diff --git a/include/scrimmage/plugins/autonomy/TakeFlag/TakeFlag.h b/include/scrimmage/plugins/autonomy/TakeFlag/TakeFlag.h
index 8e99749b8a..dfa1d12ab2 100644
--- a/include/scrimmage/plugins/autonomy/TakeFlag/TakeFlag.h
+++ b/include/scrimmage/plugins/autonomy/TakeFlag/TakeFlag.h
@@ -33,14 +33,13 @@
 #ifndef INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_TAKEFLAG_TAKEFLAG_H_
 #define INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_TAKEFLAG_TAKEFLAG_H_
 #include <scrimmage/autonomy/Autonomy.h>
-
 #include <scrimmage/plugins/interaction/Boundary/BoundaryBase.h>
 #include <scrimmage/proto/Shape.pb.h>
 
-#include <string>
 #include <map>
-#include <utility>
 #include <memory>
+#include <string>
+#include <utility>
 
 namespace scrimmage {
 
@@ -52,15 +51,15 @@ namespace autonomy {
 class TakeFlag : public scrimmage::Autonomy {
  public:
     TakeFlag();
-    void init(std::map<std::string, std::string> &params) override;
+    void init(std::map<std::string, std::string>& params) override;
     bool step_autonomy(double t, double dt) override;
 
  protected:
     int flag_boundary_id_ = -1;
     int capture_boundary_id_ = -1;
 
-    std::map<int, std::pair<scrimmage_proto::Shape,
-        std::shared_ptr<interaction::BoundaryBase>>> boundaries_;
+    std::map<int, std::pair<scrimmage_proto::Shape, std::shared_ptr<interaction::BoundaryBase>>>
+        boundaries_;
 
     bool has_flag_ = false;
 
@@ -68,6 +67,6 @@ class TakeFlag : public scrimmage::Autonomy {
     int output_vel_y_idx_ = 0;
     int output_vel_z_idx_ = 0;
 };
-} // namespace autonomy
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_TAKEFLAG_TAKEFLAG_H_
+}  // namespace autonomy
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_TAKEFLAG_TAKEFLAG_H_
diff --git a/include/scrimmage/plugins/autonomy/TrailMS/TrailMS.h b/include/scrimmage/plugins/autonomy/TrailMS/TrailMS.h
index 51f449a83c..0f5751436f 100644
--- a/include/scrimmage/plugins/autonomy/TrailMS/TrailMS.h
+++ b/include/scrimmage/plugins/autonomy/TrailMS/TrailMS.h
@@ -34,14 +34,14 @@
 #define INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_TRAILMS_TRAILMS_H_
 #include <scrimmage/plugins/autonomy/MotorSchemas/BehaviorBase.h>
 
-#include <string>
 #include <map>
+#include <string>
 
 namespace scrimmage {
 namespace autonomy {
 class TrailMS : public scrimmage::autonomy::motor_schemas::BehaviorBase {
  public:
-    void init(std::map<std::string, std::string> &params) override;
+    void init(std::map<std::string, std::string>& params) override;
     bool step_autonomy(double t, double dt) override;
 
  protected:
@@ -54,6 +54,6 @@ class TrailMS : public scrimmage::autonomy::motor_schemas::BehaviorBase {
 
     scrimmage_proto::ShapePtr sphere_shape_;
 };
-} // namespace autonomy
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_TRAILMS_TRAILMS_H_
+}  // namespace autonomy
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_TRAILMS_TRAILMS_H_
diff --git a/include/scrimmage/plugins/autonomy/TrajectoryRecordPlayback/TrajectoryPoint.h b/include/scrimmage/plugins/autonomy/TrajectoryRecordPlayback/TrajectoryPoint.h
index 66bdf86c0a..fb05c76186 100644
--- a/include/scrimmage/plugins/autonomy/TrajectoryRecordPlayback/TrajectoryPoint.h
+++ b/include/scrimmage/plugins/autonomy/TrajectoryRecordPlayback/TrajectoryPoint.h
@@ -40,24 +40,39 @@ namespace scrimmage {
 namespace autonomy {
 class TrajectoryPoint {
  public:
-    TrajectoryPoint() : t_(0) {}
-    TrajectoryPoint(double t, scrimmage::State &state, scrimmage::State &desired)
-        : t_(t), state_(state), desired_state_(desired) { }
+    TrajectoryPoint() : t_(0) {
+    }
+    TrajectoryPoint(double t, scrimmage::State& state, scrimmage::State& desired)
+        : t_(t), state_(state), desired_state_(desired) {
+    }
 
-    double t() { return t_; }
-    scrimmage::State &state() { return state_; }
-    scrimmage::State &desired_state() { return desired_state_; }
+    double t() {
+        return t_;
+    }
+    scrimmage::State& state() {
+        return state_;
+    }
+    scrimmage::State& desired_state() {
+        return desired_state_;
+    }
 
-    void set_t(double t) { t_ = t; }
-    void set_state(scrimmage::State &state) { state_ = state; }
-    void set_desired_state(scrimmage::State &state) { desired_state_ = state; }
+    void set_t(double t) {
+        t_ = t;
+    }
+    void set_state(scrimmage::State& state) {
+        state_ = state;
+    }
+    void set_desired_state(scrimmage::State& state) {
+        desired_state_ = state;
+    }
 
  protected:
     double t_;
     scrimmage::State state_;
     scrimmage::State desired_state_;
+
  private:
 };
-} // namespace autonomy
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_TRAJECTORYRECORDPLAYBACK_TRAJECTORYPOINT_H_
+}  // namespace autonomy
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_TRAJECTORYRECORDPLAYBACK_TRAJECTORYPOINT_H_
diff --git a/include/scrimmage/plugins/autonomy/TrajectoryRecordPlayback/TrajectoryRecordPlayback.h b/include/scrimmage/plugins/autonomy/TrajectoryRecordPlayback/TrajectoryRecordPlayback.h
index 70cc8bc9ae..add79fcc50 100644
--- a/include/scrimmage/plugins/autonomy/TrajectoryRecordPlayback/TrajectoryRecordPlayback.h
+++ b/include/scrimmage/plugins/autonomy/TrajectoryRecordPlayback/TrajectoryRecordPlayback.h
@@ -48,8 +48,9 @@ class TrajectoryRecordPlayback : public scrimmage::Autonomy {
  public:
     TrajectoryRecordPlayback();
     ~TrajectoryRecordPlayback();
-    void init(std::map<std::string, std::string> &params) override;
+    void init(std::map<std::string, std::string>& params) override;
     bool step_autonomy(double t, double dt) override;
+
  protected:
     std::list<TrajectoryPoint> trajs_;
     std::list<TrajectoryPoint>::iterator it_traj_;
@@ -61,6 +62,6 @@ class TrajectoryRecordPlayback : public scrimmage::Autonomy {
 
  private:
 };
-} // namespace autonomy
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_TRAJECTORYRECORDPLAYBACK_TRAJECTORYRECORDPLAYBACK_H_
+}  // namespace autonomy
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_TRAJECTORYRECORDPLAYBACK_TRAJECTORYRECORDPLAYBACK_H_
diff --git a/include/scrimmage/plugins/autonomy/WaypointDispatcher/WaypointDispatcher.h b/include/scrimmage/plugins/autonomy/WaypointDispatcher/WaypointDispatcher.h
index fc438612ab..a0094bc333 100644
--- a/include/scrimmage/plugins/autonomy/WaypointDispatcher/WaypointDispatcher.h
+++ b/include/scrimmage/plugins/autonomy/WaypointDispatcher/WaypointDispatcher.h
@@ -36,20 +36,20 @@
 #include <scrimmage/common/Waypoint.h>
 #include <scrimmage/plugins/autonomy/WaypointGenerator/WaypointList.h>
 
-#include <string>
-#include <map>
 #include <list>
+#include <map>
 #include <memory>
+#include <string>
 
 namespace scrimmage {
 namespace autonomy {
 class WaypointDispatcher : public scrimmage::Autonomy {
  public:
-    void init(std::map<std::string, std::string> &params) override;
+    void init(std::map<std::string, std::string>& params) override;
     bool step_autonomy(double t, double dt) override;
 
  protected:
-    Eigen::Vector3d lla_to_xyz(const Waypoint &wpt);
+    Eigen::Vector3d lla_to_xyz(const Waypoint& wpt);
 
     WaypointList wp_list_;
     std::list<Waypoint>::iterator wp_it_;
@@ -69,6 +69,6 @@ class WaypointDispatcher : public scrimmage::Autonomy {
 
     bool tolerance_in_2d_ = false;
 };
-} // namespace autonomy
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_WAYPOINTDISPATCHER_WAYPOINTDISPATCHER_H_
+}  // namespace autonomy
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_WAYPOINTDISPATCHER_WAYPOINTDISPATCHER_H_
diff --git a/include/scrimmage/plugins/autonomy/WaypointGenerator/WaypointGenerator.h b/include/scrimmage/plugins/autonomy/WaypointGenerator/WaypointGenerator.h
index 7c5a1d56a3..396b1db13a 100644
--- a/include/scrimmage/plugins/autonomy/WaypointGenerator/WaypointGenerator.h
+++ b/include/scrimmage/plugins/autonomy/WaypointGenerator/WaypointGenerator.h
@@ -1,4 +1,4 @@
- /*!
+/*!
  * @file
  *
  * @section LICENSE
@@ -33,8 +33,8 @@
 #ifndef INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_WAYPOINTGENERATOR_WAYPOINTGENERATOR_H_
 #define INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_WAYPOINTGENERATOR_WAYPOINTGENERATOR_H_
 
-#include <scrimmage/plugins/autonomy/WaypointGenerator/WaypointList.h>
 #include <scrimmage/autonomy/Autonomy.h>
+#include <scrimmage/plugins/autonomy/WaypointGenerator/WaypointList.h>
 
 #include <map>
 #include <string>
@@ -45,9 +45,9 @@ namespace autonomy {
 class WaypointGenerator : public scrimmage::Autonomy {
  public:
     WaypointGenerator();
-    void init(std::map<std::string, std::string> &params) override;
+    void init(std::map<std::string, std::string>& params) override;
     bool step_autonomy(double t, double dt) override;
-    void draw_waypoints(WaypointList &wp_list);
+    void draw_waypoints(WaypointList& wp_list);
 
  protected:
     WaypointList wp_list_;
@@ -60,6 +60,6 @@ class WaypointGenerator : public scrimmage::Autonomy {
     uint8_t position_y_idx_ = 0;
     uint8_t position_z_idx_ = 0;
 };
-} // namespace autonomy
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_WAYPOINTGENERATOR_WAYPOINTGENERATOR_H_
+}  // namespace autonomy
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_WAYPOINTGENERATOR_WAYPOINTGENERATOR_H_
diff --git a/include/scrimmage/plugins/autonomy/WaypointGenerator/WaypointList.h b/include/scrimmage/plugins/autonomy/WaypointGenerator/WaypointList.h
index 77de27f936..d2731385ce 100644
--- a/include/scrimmage/plugins/autonomy/WaypointGenerator/WaypointList.h
+++ b/include/scrimmage/plugins/autonomy/WaypointGenerator/WaypointList.h
@@ -42,19 +42,20 @@ namespace autonomy {
 
 class WaypointList {
  public:
-    enum class WaypointMode {
-        follow_once = 0,
-        back_and_forth = 1,
-        loiter = 2,
-        racetrack = 3
-    };
+    enum class WaypointMode { follow_once = 0, back_and_forth = 1, loiter = 2, racetrack = 3 };
 
     WaypointList() {
     }
 
-    std::list<Waypoint> & waypoints() { return waypoints_; }
-    void set_mode(WaypointMode mode) { mode_ = mode; }
-    WaypointMode mode() { return mode_; }
+    std::list<Waypoint>& waypoints() {
+        return waypoints_;
+    }
+    void set_mode(WaypointMode mode) {
+        mode_ = mode;
+    }
+    WaypointMode mode() {
+        return mode_;
+    }
 
     friend std::ostream& operator<<(std::ostream& os, WaypointList& wp_list) {
         for (Waypoint wp : wp_list.waypoints()) {
@@ -80,6 +81,6 @@ class WaypointList {
     WaypointMode mode_ = WaypointMode::follow_once;
     unsigned int cycles_ = 1;
 };
-} // namespace autonomy
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_WAYPOINTGENERATOR_WAYPOINTLIST_H_
+}  // namespace autonomy
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_WAYPOINTGENERATOR_WAYPOINTLIST_H_
diff --git a/include/scrimmage/plugins/autonomy/follow/Follow.h b/include/scrimmage/plugins/autonomy/follow/Follow.h
index b4ad16b8d5..c0dc739f58 100644
--- a/include/scrimmage/plugins/autonomy/follow/Follow.h
+++ b/include/scrimmage/plugins/autonomy/follow/Follow.h
@@ -35,20 +35,19 @@
 
 #include <scrimmage/autonomy/Autonomy.h>
 
-#include <string>
-#include <map>
-
-#include <GeographicLib/Geodesic.hpp>
 #include <GeographicLib/Constants.hpp>
+#include <GeographicLib/Geodesic.hpp>
+#include <map>
+#include <string>
 
 namespace scrimmage {
 namespace autonomy {
-class Follow : public scrimmage::Autonomy{
+class Follow : public scrimmage::Autonomy {
  public:
-     Follow();
-     void init(std::map<std::string, std::string> &params) override;
-     bool step_autonomy(double t, double dt) override;
+    Follow();
+    void init(std::map<std::string, std::string>& params) override;
+    bool step_autonomy(double t, double dt) override;
 };
-} // namespace autonomy
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_FOLLOW_FOLLOW_H_
+}  // namespace autonomy
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_FOLLOW_FOLLOW_H_
diff --git a/include/scrimmage/plugins/controller/AircraftPIDController/AircraftPIDController.h b/include/scrimmage/plugins/controller/AircraftPIDController/AircraftPIDController.h
index 7fbe02a48e..3618d245a8 100644
--- a/include/scrimmage/plugins/controller/AircraftPIDController/AircraftPIDController.h
+++ b/include/scrimmage/plugins/controller/AircraftPIDController/AircraftPIDController.h
@@ -33,11 +33,10 @@
 #ifndef INCLUDE_SCRIMMAGE_PLUGINS_CONTROLLER_AIRCRAFTPIDCONTROLLER_AIRCRAFTPIDCONTROLLER_H_
 #define INCLUDE_SCRIMMAGE_PLUGINS_CONTROLLER_AIRCRAFTPIDCONTROLLER_AIRCRAFTPIDCONTROLLER_H_
 
-#include <scrimmage/motion/Controller.h>
 #include <scrimmage/common/PID.h>
+#include <scrimmage/motion/Controller.h>
 
 #include <Eigen/Dense>
-
 #include <map>
 #include <string>
 
@@ -47,7 +46,7 @@ namespace controller {
 class AircraftPIDController : public scrimmage::Controller {
  public:
     AircraftPIDController();
-    void init(std::map<std::string, std::string> &params) override;
+    void init(std::map<std::string, std::string>& params) override;
     bool step(double t, double dt) override;
 
  protected:
@@ -75,6 +74,6 @@ class AircraftPIDController : public scrimmage::Controller {
 
     bool use_roll_control_ = false;
 };
-} // namespace controller
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_CONTROLLER_AIRCRAFTPIDCONTROLLER_AIRCRAFTPIDCONTROLLER_H_
+}  // namespace controller
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_CONTROLLER_AIRCRAFTPIDCONTROLLER_AIRCRAFTPIDCONTROLLER_H_
diff --git a/include/scrimmage/plugins/controller/AircraftToSingleIntegratorController/AircraftToSingleIntegratorController.h b/include/scrimmage/plugins/controller/AircraftToSingleIntegratorController/AircraftToSingleIntegratorController.h
index 01f690a77e..9de112fb80 100644
--- a/include/scrimmage/plugins/controller/AircraftToSingleIntegratorController/AircraftToSingleIntegratorController.h
+++ b/include/scrimmage/plugins/controller/AircraftToSingleIntegratorController/AircraftToSingleIntegratorController.h
@@ -43,7 +43,8 @@ namespace controller {
 
 class AircraftToSingleIntegratorController : public Controller {
  public:
-    virtual void init(std::map<std::string, std::string> &params) {}
+    virtual void init(std::map<std::string, std::string>& params) {
+    }
     virtual bool step(double t, double dt) {
         double desired_heading = desired_state_->quat().yaw();
         double desired_alt = desired_state_->pos()(2);
@@ -57,7 +58,9 @@ class AircraftToSingleIntegratorController : public Controller {
         return true;
     }
 
-    virtual Eigen::Vector3d &u() { return u_; }
+    virtual Eigen::Vector3d& u() {
+        return u_;
+    }
 
  protected:
     Eigen::Vector3d u_;
diff --git a/include/scrimmage/plugins/controller/DirectController/DirectController.h b/include/scrimmage/plugins/controller/DirectController/DirectController.h
index ab1ca791df..b873345f38 100644
--- a/include/scrimmage/plugins/controller/DirectController/DirectController.h
+++ b/include/scrimmage/plugins/controller/DirectController/DirectController.h
@@ -36,7 +36,6 @@
 #include <scrimmage/motion/Controller.h>
 
 #include <Eigen/Dense>
-
 #include <map>
 #include <string>
 
@@ -45,13 +44,14 @@ namespace controller {
 
 class DirectController : public scrimmage::Controller {
  public:
-    void init(std::map<std::string, std::string> &params) override;
+    void init(std::map<std::string, std::string>& params) override;
     bool step(double t, double dt) override;
+
  protected:
     // Key : Output variable index determined by motion model
     // Value: Input variable index
     std::map<int, int> io_map_;
 };
-} // namespace controller
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_CONTROLLER_DIRECTCONTROLLER_DIRECTCONTROLLER_H_
+}  // namespace controller
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_CONTROLLER_DIRECTCONTROLLER_DIRECTCONTROLLER_H_
diff --git a/include/scrimmage/plugins/controller/DoubleIntegratorControllerVelYaw/DoubleIntegratorControllerVelYaw.h b/include/scrimmage/plugins/controller/DoubleIntegratorControllerVelYaw/DoubleIntegratorControllerVelYaw.h
index 5a82865223..d413662dbf 100644
--- a/include/scrimmage/plugins/controller/DoubleIntegratorControllerVelYaw/DoubleIntegratorControllerVelYaw.h
+++ b/include/scrimmage/plugins/controller/DoubleIntegratorControllerVelYaw/DoubleIntegratorControllerVelYaw.h
@@ -37,14 +37,14 @@
 #include <scrimmage/plugins/motion/DoubleIntegrator/DoubleIntegrator.h>
 
 #include <map>
-#include <vector>
 #include <string>
+#include <vector>
 
 namespace scrimmage {
 namespace controller {
 class DoubleIntegratorControllerVelYaw : public scrimmage::Controller {
  public:
-    void init(std::map<std::string, std::string> &params) override;
+    void init(std::map<std::string, std::string>& params) override;
     bool step(double t, double dt) override;
 
  protected:
@@ -62,6 +62,6 @@ class DoubleIntegratorControllerVelYaw : public scrimmage::Controller {
     int acc_z_idx_ = 0;
     int turn_rate_idx_ = 0;
 };
-} // namespace controller
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_CONTROLLER_DOUBLEINTEGRATORCONTROLLERVELYAW_DOUBLEINTEGRATORCONTROLLERVELYAW_H_
+}  // namespace controller
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_CONTROLLER_DOUBLEINTEGRATORCONTROLLERVELYAW_DOUBLEINTEGRATORCONTROLLERVELYAW_H_
diff --git a/include/scrimmage/plugins/controller/DoubleIntegratorControllerWaypoint/DoubleIntegratorControllerWaypoint.h b/include/scrimmage/plugins/controller/DoubleIntegratorControllerWaypoint/DoubleIntegratorControllerWaypoint.h
index d3da126bec..c7d4c143d2 100644
--- a/include/scrimmage/plugins/controller/DoubleIntegratorControllerWaypoint/DoubleIntegratorControllerWaypoint.h
+++ b/include/scrimmage/plugins/controller/DoubleIntegratorControllerWaypoint/DoubleIntegratorControllerWaypoint.h
@@ -42,7 +42,7 @@ namespace scrimmage {
 namespace controller {
 class DoubleIntegratorControllerWaypoint : public scrimmage::Controller {
  public:
-    void init(std::map<std::string, std::string> &params) override;
+    void init(std::map<std::string, std::string>& params) override;
     bool step(double t, double dt) override;
 
  protected:
diff --git a/include/scrimmage/plugins/controller/FixedWing6DOFControllerPID/FixedWing6DOFControllerPID.h b/include/scrimmage/plugins/controller/FixedWing6DOFControllerPID/FixedWing6DOFControllerPID.h
index b36650cdc8..4740eb6544 100644
--- a/include/scrimmage/plugins/controller/FixedWing6DOFControllerPID/FixedWing6DOFControllerPID.h
+++ b/include/scrimmage/plugins/controller/FixedWing6DOFControllerPID/FixedWing6DOFControllerPID.h
@@ -33,8 +33,8 @@
 #ifndef INCLUDE_SCRIMMAGE_PLUGINS_CONTROLLER_FIXEDWING6DOFCONTROLLERPID_FIXEDWING6DOFCONTROLLERPID_H_
 #define INCLUDE_SCRIMMAGE_PLUGINS_CONTROLLER_FIXEDWING6DOFCONTROLLERPID_FIXEDWING6DOFCONTROLLERPID_H_
 
-#include <scrimmage/motion/Controller.h>
 #include <scrimmage/common/PID.h>
+#include <scrimmage/motion/Controller.h>
 
 #include <map>
 #include <string>
@@ -43,7 +43,7 @@ namespace scrimmage {
 namespace controller {
 class FixedWing6DOFControllerPID : public Controller {
  public:
-    virtual void init(std::map<std::string, std::string> &params);
+    virtual void init(std::map<std::string, std::string>& params);
     virtual bool step(double t, double dt);
 
  protected:
@@ -56,6 +56,6 @@ class FixedWing6DOFControllerPID : public Controller {
     int aileron_idx_ = 0;
     int rudder_idx_ = 0;
 };
-} // namespace controller
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_CONTROLLER_FIXEDWING6DOFCONTROLLERPID_FIXEDWING6DOFCONTROLLERPID_H_
+}  // namespace controller
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_CONTROLLER_FIXEDWING6DOFCONTROLLERPID_FIXEDWING6DOFCONTROLLERPID_H_
diff --git a/include/scrimmage/plugins/controller/FixedWing6DOFControllerPID/FixedWingControllerPID.h b/include/scrimmage/plugins/controller/FixedWing6DOFControllerPID/FixedWingControllerPID.h
index 095cbc215f..163f0f473e 100644
--- a/include/scrimmage/plugins/controller/FixedWing6DOFControllerPID/FixedWingControllerPID.h
+++ b/include/scrimmage/plugins/controller/FixedWing6DOFControllerPID/FixedWingControllerPID.h
@@ -36,16 +36,18 @@
 #include <scrimmage/plugins/motion/FixedWing6DOF/FixedWing6DOF.h>
 
 #include <map>
-#include <string>
 #include <memory>
+#include <string>
 
 namespace scrimmage {
 namespace controller {
 class FixedWing6DOFControllerPID : public FixedWing6DOF::Controller {
  public:
-    virtual void init(std::map<std::string, std::string> &params);
+    virtual void init(std::map<std::string, std::string>& params);
     virtual bool step(double t, double dt);
-    virtual std::shared_ptr<Eigen::Vector4d> u() {return u_;}
+    virtual std::shared_ptr<Eigen::Vector4d> u() {
+        return u_;
+    }
 
  protected:
     std::shared_ptr<Eigen::Vector4d> u_;
@@ -53,6 +55,6 @@ class FixedWing6DOFControllerPID : public FixedWing6DOF::Controller {
     scrimmage::PID alt_pid_;
     scrimmage::PID vel_pid_;
 };
-} // namespace controller
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_CONTROLLER_FIXEDWING6DOFCONTROLLERPID_FIXEDWINGCONTROLLERPID_H_
+}  // namespace controller
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_CONTROLLER_FIXEDWING6DOFCONTROLLERPID_FIXEDWINGCONTROLLERPID_H_
diff --git a/include/scrimmage/plugins/controller/FixedWing6DOFControllerROS/FixedWing6DOFControllerROS.h b/include/scrimmage/plugins/controller/FixedWing6DOFControllerROS/FixedWing6DOFControllerROS.h
index 227fbe82ac..461fe6ab41 100644
--- a/include/scrimmage/plugins/controller/FixedWing6DOFControllerROS/FixedWing6DOFControllerROS.h
+++ b/include/scrimmage/plugins/controller/FixedWing6DOFControllerROS/FixedWing6DOFControllerROS.h
@@ -33,10 +33,9 @@
 #ifndef INCLUDE_SCRIMMAGE_PLUGINS_CONTROLLER_FIXEDWING6DOFCONTROLLERROS_FIXEDWING6DOFCONTROLLERROS_H_
 #define INCLUDE_SCRIMMAGE_PLUGINS_CONTROLLER_FIXEDWING6DOFCONTROLLERROS_FIXEDWING6DOFCONTROLLERROS_H_
 
-#include <scrimmage/plugins/motion/FixedWing6DOF/FixedWing6DOF.h>
-
 #include <geometry_msgs/Twist.h>
 #include <ros/ros.h>
+#include <scrimmage/plugins/motion/FixedWing6DOF/FixedWing6DOF.h>
 
 #include <map>
 #include <memory>
diff --git a/include/scrimmage/plugins/controller/HarmonicOscillatorConstController/HarmonicOscillatorConstController.h b/include/scrimmage/plugins/controller/HarmonicOscillatorConstController/HarmonicOscillatorConstController.h
index 88cb5b43f2..fbfe7b6a60 100644
--- a/include/scrimmage/plugins/controller/HarmonicOscillatorConstController/HarmonicOscillatorConstController.h
+++ b/include/scrimmage/plugins/controller/HarmonicOscillatorConstController/HarmonicOscillatorConstController.h
@@ -42,7 +42,7 @@ namespace controller {
 
 class HarmonicOscillatorConstController : public scrimmage::Controller {
  public:
-    void init(std::map<std::string, std::string> &params) override;
+    void init(std::map<std::string, std::string>& params) override;
     bool step(double t, double dt) override;
 
  protected:
@@ -51,6 +51,6 @@ class HarmonicOscillatorConstController : public scrimmage::Controller {
     // input to motion model
     double acceleration_z_ = 0;
 };
-} // namespace controller
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_CONTROLLER_HARMONICOSCILLATORCONSTCONTROLLER_HARMONICOSCILLATORCONSTCONTROLLER_H_
+}  // namespace controller
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_CONTROLLER_HARMONICOSCILLATORCONSTCONTROLLER_HARMONICOSCILLATORCONSTCONTROLLER_H_
diff --git a/include/scrimmage/plugins/controller/JSBSimControlControllerHeadingPID/JSBSimControlControllerHeadingPID.h b/include/scrimmage/plugins/controller/JSBSimControlControllerHeadingPID/JSBSimControlControllerHeadingPID.h
index fbd1430078..97e0c92652 100644
--- a/include/scrimmage/plugins/controller/JSBSimControlControllerHeadingPID/JSBSimControlControllerHeadingPID.h
+++ b/include/scrimmage/plugins/controller/JSBSimControlControllerHeadingPID/JSBSimControlControllerHeadingPID.h
@@ -33,9 +33,9 @@
 #ifndef INCLUDE_SCRIMMAGE_PLUGINS_CONTROLLER_JSBSIMCONTROLCONTROLLERHEADINGPID_JSBSIMCONTROLCONTROLLERHEADINGPID_H_
 #define INCLUDE_SCRIMMAGE_PLUGINS_CONTROLLER_JSBSIMCONTROLCONTROLLERHEADINGPID_JSBSIMCONTROLCONTROLLERHEADINGPID_H_
 
-#include <scrimmage/motion/Controller.h>
 #include <scrimmage/common/PID.h>
 #include <scrimmage/math/Angles.h>
+#include <scrimmage/motion/Controller.h>
 
 #include <map>
 #include <string>
@@ -44,7 +44,7 @@ namespace scrimmage {
 namespace controller {
 class JSBSimControlControllerHeadingPID : public scrimmage::Controller {
  public:
-    void init(std::map<std::string, std::string> &params) override;
+    void init(std::map<std::string, std::string>& params) override;
     bool step(double t, double dt) override;
 
  protected:
@@ -64,6 +64,6 @@ class JSBSimControlControllerHeadingPID : public scrimmage::Controller {
     int aileron_idx_ = 0;
     int rudder_idx_ = 0;
 };
-} // namespace controller
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_CONTROLLER_JSBSIMCONTROLCONTROLLERHEADINGPID_JSBSIMCONTROLCONTROLLERHEADINGPID_H_
+}  // namespace controller
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_CONTROLLER_JSBSIMCONTROLCONTROLLERHEADINGPID_JSBSIMCONTROLCONTROLLERHEADINGPID_H_
diff --git a/include/scrimmage/plugins/controller/JSBSimModelControllerDirect/JSBSimModelControllerDirect.h b/include/scrimmage/plugins/controller/JSBSimModelControllerDirect/JSBSimModelControllerDirect.h
index f054b3db33..0bf59608c2 100644
--- a/include/scrimmage/plugins/controller/JSBSimModelControllerDirect/JSBSimModelControllerDirect.h
+++ b/include/scrimmage/plugins/controller/JSBSimModelControllerDirect/JSBSimModelControllerDirect.h
@@ -43,7 +43,7 @@ namespace controller {
 
 class JSBSimModelControllerDirect : public Controller {
  public:
-    virtual void init(std::map<std::string, std::string> &params);
+    virtual void init(std::map<std::string, std::string>& params);
     virtual bool step(double t, double dt);
 
  protected:
@@ -57,6 +57,6 @@ class JSBSimModelControllerDirect : public Controller {
     int output_roll_idx_ = 0;
     int output_alt_or_pitch_idx_ = 0;
 };
-} // namespace controller
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_CONTROLLER_JSBSIMMODELCONTROLLERDIRECT_JSBSIMMODELCONTROLLERDIRECT_H_
+}  // namespace controller
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_CONTROLLER_JSBSIMMODELCONTROLLERDIRECT_JSBSIMMODELCONTROLLERDIRECT_H_
diff --git a/include/scrimmage/plugins/controller/JSBSimModelControllerHeadingPID/JSBSimModelControllerHeadingPID.h b/include/scrimmage/plugins/controller/JSBSimModelControllerHeadingPID/JSBSimModelControllerHeadingPID.h
index 70730ba1cf..d916481d36 100644
--- a/include/scrimmage/plugins/controller/JSBSimModelControllerHeadingPID/JSBSimModelControllerHeadingPID.h
+++ b/include/scrimmage/plugins/controller/JSBSimModelControllerHeadingPID/JSBSimModelControllerHeadingPID.h
@@ -45,7 +45,7 @@ namespace controller {
 
 class JSBSimModelControllerHeadingPID : public Controller {
  public:
-    virtual void init(std::map<std::string, std::string> &params);
+    virtual void init(std::map<std::string, std::string>& params);
     virtual bool step(double t, double dt);
 
  protected:
@@ -65,6 +65,6 @@ class JSBSimModelControllerHeadingPID : public Controller {
     int output_bank_idx_ = 0;
     int output_alt_idx_ = 0;
 };
-} // namespace controller
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_CONTROLLER_JSBSIMMODELCONTROLLERHEADINGPID_JSBSIMMODELCONTROLLERHEADINGPID_H_
+}  // namespace controller
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_CONTROLLER_JSBSIMMODELCONTROLLERHEADINGPID_JSBSIMMODELCONTROLLERHEADINGPID_H_
diff --git a/include/scrimmage/plugins/controller/JoystickController/AxisScale.h b/include/scrimmage/plugins/controller/JoystickController/AxisScale.h
index 30e123e77b..e9a6d07b6e 100644
--- a/include/scrimmage/plugins/controller/JoystickController/AxisScale.h
+++ b/include/scrimmage/plugins/controller/JoystickController/AxisScale.h
@@ -42,19 +42,27 @@ namespace controller {
 
 class AxisScale {
  public:
-    AxisScale(int axis_index, double input_min, double input_max,
-              double output_min, double output_max, double coeff,
-              int vector_index) : axis_index_(axis_index),
-        input_min_(input_min), input_max_(input_max),
-        output_min_(output_min), output_max_(output_max), coeff_(coeff),
-        vector_index_(vector_index) { }
+    AxisScale(int axis_index, double input_min, double input_max, double output_min,
+              double output_max, double coeff, int vector_index)
+        : axis_index_(axis_index),
+          input_min_(input_min),
+          input_max_(input_max),
+          output_min_(output_min),
+          output_max_(output_max),
+          coeff_(coeff),
+          vector_index_(vector_index) {
+    }
 
-    int axis_index() { return axis_index_; }
-    int vector_index() { return vector_index_; }
+    int axis_index() {
+        return axis_index_;
+    }
+    int vector_index() {
+        return vector_index_;
+    }
 
     double scale(double input) {
-        return coeff_ * scrimmage::scale<double>(input, input_min_, input_max_,
-                                                 output_min_, output_max_);
+        return coeff_
+               * scrimmage::scale<double>(input, input_min_, input_max_, output_min_, output_max_);
     }
 
  protected:
@@ -67,6 +75,6 @@ class AxisScale {
     int vector_index_ = 0;
 };
 
-} // namespace controller
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_CONTROLLER_JOYSTICKCONTROLLER_AXISSCALE_H_
+}  // namespace controller
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_CONTROLLER_JOYSTICKCONTROLLER_AXISSCALE_H_
diff --git a/include/scrimmage/plugins/controller/JoystickController/Joystick.h b/include/scrimmage/plugins/controller/JoystickController/Joystick.h
index 8963b85eb9..676e90d7c9 100644
--- a/include/scrimmage/plugins/controller/JoystickController/Joystick.h
+++ b/include/scrimmage/plugins/controller/JoystickController/Joystick.h
@@ -36,19 +36,18 @@
 #ifndef __APPLE__
 
 #include <fcntl.h>
-#include <unistd.h>
-#include <sys/ioctl.h>
 #include <linux/joystick.h>
-
-#include <scrimmage/plugins/controller/JoystickController/AxisScale.h>
 #include <scrimmage/common/VariableIO.h>
+#include <scrimmage/plugins/controller/JoystickController/AxisScale.h>
 #include <scrimmage/pubsub/PubSub.h>
 #include <scrimmage/pubsub/Publisher.h>
+#include <sys/ioctl.h>
+#include <unistd.h>
 
-#include <string>
 #include <list>
-#include <vector>
 #include <map>
+#include <string>
+#include <vector>
 
 namespace scrimmage {
 namespace controller {
@@ -58,17 +57,16 @@ class Joystick {
     Joystick();
     ~Joystick();
 
-    void init(std::map<std::string, std::string> &params, VariableIO &vars,
-              EntityPluginPtr plugin);
-    bool step(double t, double dt, VariableIO &vars);
+    void init(std::map<std::string, std::string>& params, VariableIO& vars, EntityPluginPtr plugin);
+    bool step(double t, double dt, VariableIO& vars);
 
  protected:
     int joy_fd_ = -1;
-    int *axis_ = NULL;
+    int* axis_ = NULL;
     int num_of_axis_ = 0;
     int num_of_buttons_ = 0;
-	char *button_ = NULL;
-	struct js_event js_;
+    char* button_ = NULL;
+    struct js_event js_;
 
     int min_value = -32767;
     int max_value = +32767;
@@ -83,9 +81,9 @@ class Joystick {
     std::vector<bool> prev_button_state_;
 };
 
-} // namespace controller
-} // namespace scrimmage
+}  // namespace controller
+}  // namespace scrimmage
 
 #endif  // __APPLE__
 
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_CONTROLLER_JOYSTICKCONTROLLER_JOYSTICK_H_
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_CONTROLLER_JOYSTICKCONTROLLER_JOYSTICK_H_
diff --git a/include/scrimmage/plugins/controller/JoystickController/JoystickController.h b/include/scrimmage/plugins/controller/JoystickController/JoystickController.h
index 89320b99a2..7d7cfa00fc 100644
--- a/include/scrimmage/plugins/controller/JoystickController/JoystickController.h
+++ b/include/scrimmage/plugins/controller/JoystickController/JoystickController.h
@@ -34,20 +34,17 @@
 #define INCLUDE_SCRIMMAGE_PLUGINS_CONTROLLER_JOYSTICKCONTROLLER_JOYSTICKCONTROLLER_H_
 
 #include <fcntl.h>
-#include <unistd.h>
-#include <sys/ioctl.h>
 #include <linux/joystick.h>
-
-#include <scrimmage/plugins/controller/JoystickController/Joystick.h>
-
-#include <scrimmage/motion/Controller.h>
 #include <scrimmage/common/Utilities.h>
+#include <scrimmage/motion/Controller.h>
+#include <scrimmage/plugins/controller/JoystickController/Joystick.h>
+#include <sys/ioctl.h>
+#include <unistd.h>
 
 #include <Eigen/Dense>
-
+#include <list>
 #include <map>
 #include <string>
-#include <list>
 
 namespace sc = scrimmage;
 
@@ -55,12 +52,12 @@ namespace scrimmage {
 namespace controller {
 class JoystickController : public scrimmage::Controller {
  public:
-    void init(std::map<std::string, std::string> &params) override;
+    void init(std::map<std::string, std::string>& params) override;
     bool step(double t, double dt) override;
 
  protected:
     Joystick joystick_;
 };
-} // namespace controller
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_CONTROLLER_JOYSTICKCONTROLLER_JOYSTICKCONTROLLER_H_
+}  // namespace controller
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_CONTROLLER_JOYSTICKCONTROLLER_JOYSTICKCONTROLLER_H_
diff --git a/include/scrimmage/plugins/controller/MotionBattery/MotionBattery.h b/include/scrimmage/plugins/controller/MotionBattery/MotionBattery.h
index 15cae8cf01..b1ed1d3564 100644
--- a/include/scrimmage/plugins/controller/MotionBattery/MotionBattery.h
+++ b/include/scrimmage/plugins/controller/MotionBattery/MotionBattery.h
@@ -33,12 +33,12 @@
 #ifndef INCLUDE_SCRIMMAGE_PLUGINS_CONTROLLER_MOTIONBATTERY_MOTIONBATTERY_H_
 #define INCLUDE_SCRIMMAGE_PLUGINS_CONTROLLER_MOTIONBATTERY_MOTIONBATTERY_H_
 
-#include <scrimmage/motion/Controller.h>
 #include <scrimmage/common/Battery.h>
+#include <scrimmage/motion/Controller.h>
 
 #include <map>
-#include <string>
 #include <memory>
+#include <string>
 
 namespace scrimmage {
 namespace controller {
@@ -47,29 +47,32 @@ class MotionBattery : public scrimmage::Controller {
  public:
     // enum class DepletionEquation {LINEAR};
 
-    void init(std::map<std::string, std::string> &params) override;
+    void init(std::map<std::string, std::string>& params) override;
     bool step(double t, double dt) override;
 
  protected:
-    bool get_battery_charge(scrimmage::MessageBasePtr request,
-                            scrimmage::MessageBasePtr &response);
-    double calculate_charge_usage(const double &throttle, const double &dt);
+    bool get_battery_charge(scrimmage::MessageBasePtr request, scrimmage::MessageBasePtr& response);
+    double calculate_charge_usage(const double& throttle, const double& dt);
 
     Battery battery_;
 
     class VarLimit {
      public:
-        VarLimit(const int &input, const int &output, const double &rate,
-                 const double &value, const bool &lim) :
-            input_idx(input), output_idx(output), depletion_rate(rate),
-            value_when_depleted(value), limit_when_depleted(lim) {}
+        VarLimit(const int& input, const int& output, const double& rate, const double& value,
+                 const bool& lim)
+            : input_idx(input),
+              output_idx(output),
+              depletion_rate(rate),
+              value_when_depleted(value),
+              limit_when_depleted(lim) {
+        }
         int input_idx = 0;
         int output_idx = 0;
         double depletion_rate = 0;
         double value_when_depleted = 0;
         bool limit_when_depleted = false;
 
-        double calc_depletion(const double &input, const double &dt) {
+        double calc_depletion(const double& input, const double& dt) {
             return input * depletion_rate * dt;
         }
     };
@@ -80,6 +83,6 @@ class MotionBattery : public scrimmage::Controller {
     bool publish_charge_ = false;
     PublisherPtr pub_charge_percentage_;
 };
-} // namespace controller
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_CONTROLLER_MOTIONBATTERY_MOTIONBATTERY_H_
+}  // namespace controller
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_CONTROLLER_MOTIONBATTERY_MOTIONBATTERY_H_
diff --git a/include/scrimmage/plugins/controller/MultirotorControllerOmega/MultirotorControllerOmega.h b/include/scrimmage/plugins/controller/MultirotorControllerOmega/MultirotorControllerOmega.h
index 4ca49ed658..e5d7686178 100644
--- a/include/scrimmage/plugins/controller/MultirotorControllerOmega/MultirotorControllerOmega.h
+++ b/include/scrimmage/plugins/controller/MultirotorControllerOmega/MultirotorControllerOmega.h
@@ -46,9 +46,11 @@ namespace controller {
 class MultirotorControllerOmega : public motion::Multirotor::Controller {
  public:
     MultirotorControllerOmega();
-    void init(std::map<std::string, std::string> &params) override;
+    void init(std::map<std::string, std::string>& params) override;
     bool step(double t, double dt) override;
-    Eigen::VectorXd &u() override { return u_; }
+    Eigen::VectorXd& u() override {
+        return u_;
+    }
 
  protected:
     Eigen::VectorXd u_;
diff --git a/include/scrimmage/plugins/controller/MultirotorControllerPID/MultirotorControllerPID.h b/include/scrimmage/plugins/controller/MultirotorControllerPID/MultirotorControllerPID.h
index b00ccbf624..afd50d8d86 100644
--- a/include/scrimmage/plugins/controller/MultirotorControllerPID/MultirotorControllerPID.h
+++ b/include/scrimmage/plugins/controller/MultirotorControllerPID/MultirotorControllerPID.h
@@ -48,9 +48,11 @@ namespace controller {
 class MultirotorControllerPID : public scrimmage::Controller {
  public:
     MultirotorControllerPID();
-    void init(std::map<std::string, std::string> &params) override;
+    void init(std::map<std::string, std::string>& params) override;
     bool step(double t, double dt) override;
-    virtual Eigen::VectorXd &u() { return u_; }
+    virtual Eigen::VectorXd& u() {
+        return u_;
+    }
 
  protected:
     Eigen::VectorXd u_;
diff --git a/include/scrimmage/plugins/controller/RigidBody6DOFControllerPID/RigidBody6DOFControllerPID.h b/include/scrimmage/plugins/controller/RigidBody6DOFControllerPID/RigidBody6DOFControllerPID.h
index 43d6ade3e5..32a269817f 100644
--- a/include/scrimmage/plugins/controller/RigidBody6DOFControllerPID/RigidBody6DOFControllerPID.h
+++ b/include/scrimmage/plugins/controller/RigidBody6DOFControllerPID/RigidBody6DOFControllerPID.h
@@ -36,16 +36,18 @@
 #include <scrimmage/plugins/motion/RigidBody6DOF/RigidBody6DOF.h>
 
 #include <map>
-#include <string>
 #include <memory>
+#include <string>
 
 namespace scrimmage {
 namespace controller {
 class RigidBody6DOFControllerPID : public motion::RigidBody6DOF::Controller {
  public:
-    void init(std::map<std::string, std::string> &params) override;
+    void init(std::map<std::string, std::string>& params) override;
     bool step(double t, double dt) override;
-    std::shared_ptr<Eigen::Vector4d> u() override {return u_;}
+    std::shared_ptr<Eigen::Vector4d> u() override {
+        return u_;
+    }
 
  protected:
     std::shared_ptr<Eigen::Vector4d> u_;
@@ -53,6 +55,6 @@ class RigidBody6DOFControllerPID : public motion::RigidBody6DOF::Controller {
     scrimmage::PID alt_pid_;
     scrimmage::PID vel_pid_;
 };
-} // namespace controller
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_CONTROLLER_RIGIDBODY6DOFCONTROLLERPID_RIGIDBODY6DOFCONTROLLERPID_H_
+}  // namespace controller
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_CONTROLLER_RIGIDBODY6DOFCONTROLLERPID_RIGIDBODY6DOFCONTROLLERPID_H_
diff --git a/include/scrimmage/plugins/controller/RigidBody6DOFControllerROS/RigidBody6DOFControllerROS.h b/include/scrimmage/plugins/controller/RigidBody6DOFControllerROS/RigidBody6DOFControllerROS.h
index 54b07011ad..202c99e02b 100644
--- a/include/scrimmage/plugins/controller/RigidBody6DOFControllerROS/RigidBody6DOFControllerROS.h
+++ b/include/scrimmage/plugins/controller/RigidBody6DOFControllerROS/RigidBody6DOFControllerROS.h
@@ -33,22 +33,23 @@
 #ifndef INCLUDE_SCRIMMAGE_PLUGINS_CONTROLLER_RIGIDBODY6DOFCONTROLLERROS_RIGIDBODY6DOFCONTROLLERROS_H_
 #define INCLUDE_SCRIMMAGE_PLUGINS_CONTROLLER_RIGIDBODY6DOFCONTROLLERROS_RIGIDBODY6DOFCONTROLLERROS_H_
 
-#include <scrimmage/plugins/motion/RigidBody6DOF/RigidBody6DOF.h>
-
-#include <ros/ros.h>
 #include <geometry_msgs/Twist.h>
+#include <ros/ros.h>
+#include <scrimmage/plugins/motion/RigidBody6DOF/RigidBody6DOF.h>
 
 #include <map>
-#include <string>
 #include <memory>
+#include <string>
 
 namespace scrimmage {
 namespace controller {
 class RigidBody6DOFControllerROS : public scrimmage::motion::RigidBody6DOF::Controller {
  public:
-    void init(std::map<std::string, std::string> &params) override;
+    void init(std::map<std::string, std::string>& params) override;
     bool step(double t, double dt) override;
-    std::shared_ptr<Eigen::Vector4d> u() override {return u_;}
+    std::shared_ptr<Eigen::Vector4d> u() override {
+        return u_;
+    }
 
  protected:
     std::shared_ptr<Eigen::Vector4d> u_;
@@ -58,6 +59,6 @@ class RigidBody6DOFControllerROS : public scrimmage::motion::RigidBody6DOF::Cont
     ros::Subscriber cmd_vel_sub_;
     geometry_msgs::Twist cmd_vel_;
 };
-} // namespace controller
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_CONTROLLER_RIGIDBODY6DOFCONTROLLERROS_RIGIDBODY6DOFCONTROLLERROS_H_
+}  // namespace controller
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_CONTROLLER_RIGIDBODY6DOFCONTROLLERROS_RIGIDBODY6DOFCONTROLLERROS_H_
diff --git a/include/scrimmage/plugins/controller/SimpleAircraftControllerPID/SimpleAircraftControllerPID.h b/include/scrimmage/plugins/controller/SimpleAircraftControllerPID/SimpleAircraftControllerPID.h
index df928bca63..0d74210e08 100644
--- a/include/scrimmage/plugins/controller/SimpleAircraftControllerPID/SimpleAircraftControllerPID.h
+++ b/include/scrimmage/plugins/controller/SimpleAircraftControllerPID/SimpleAircraftControllerPID.h
@@ -46,13 +46,14 @@ namespace controller {
  * Designed to control the simple aircraft model.
  * Horizontal channel controlled through roll rate or heading rate
  * Vertical channel controll through altitude or glide slope (tan(z_vel / sqrt(x_vel^2 + y_vel^2)))
- * Note that pitch for the simple aircraft model is actually in the direction of the velocity vector in the vertical plane (angle of attack assumed to be zero)
- * Speed controlled through a separate channel.
- * A glider can be controlled through here if the speed is always set to match the glide speed based on the roll and pitch angles.
+ * Note that pitch for the simple aircraft model is actually in the direction of the velocity vector
+ * in the vertical plane (angle of attack assumed to be zero) Speed controlled through a separate
+ * channel. A glider can be controlled through here if the speed is always set to match the glide
+ * speed based on the roll and pitch angles.
  */
 class SimpleAircraftControllerPID : public Controller {
  public:
-    virtual void init(std::map<std::string, std::string> &params);
+    virtual void init(std::map<std::string, std::string>& params);
     virtual bool step(double t, double dt);
 
  protected:
diff --git a/include/scrimmage/plugins/controller/SimpleCarControllerHeading/SimpleCarControllerHeading.h b/include/scrimmage/plugins/controller/SimpleCarControllerHeading/SimpleCarControllerHeading.h
index c4ba98987e..dbbdaa9333 100644
--- a/include/scrimmage/plugins/controller/SimpleCarControllerHeading/SimpleCarControllerHeading.h
+++ b/include/scrimmage/plugins/controller/SimpleCarControllerHeading/SimpleCarControllerHeading.h
@@ -43,7 +43,7 @@ namespace scrimmage {
 namespace controller {
 class SimpleCarControllerHeading : public Controller {
  public:
-    virtual void init(std::map<std::string, std::string> &params);
+    virtual void init(std::map<std::string, std::string>& params);
     virtual bool step(double t, double dt);
 
  protected:
@@ -55,6 +55,6 @@ class SimpleCarControllerHeading : public Controller {
     uint8_t output_vel_idx_ = 0;
     uint8_t output_turn_rate_idx_ = 0;
 };
-} // namespace controller
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_CONTROLLER_SIMPLECARCONTROLLERHEADING_SIMPLECARCONTROLLERHEADING_H_
+}  // namespace controller
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_CONTROLLER_SIMPLECARCONTROLLERHEADING_SIMPLECARCONTROLLERHEADING_H_
diff --git a/include/scrimmage/plugins/controller/SimpleQuadrotorControllerLQR/SimpleQuadrotorControllerLQR.h b/include/scrimmage/plugins/controller/SimpleQuadrotorControllerLQR/SimpleQuadrotorControllerLQR.h
index e3e656e220..484ade58dd 100644
--- a/include/scrimmage/plugins/controller/SimpleQuadrotorControllerLQR/SimpleQuadrotorControllerLQR.h
+++ b/include/scrimmage/plugins/controller/SimpleQuadrotorControllerLQR/SimpleQuadrotorControllerLQR.h
@@ -42,9 +42,11 @@ namespace scrimmage {
 namespace controller {
 class SimpleQuadrotorControllerLQR : public motion::SimpleQuadrotor::Controller {
  public:
-    void init(std::map<std::string, std::string> &params) override;
+    void init(std::map<std::string, std::string>& params) override;
     bool step(double t, double dt) override;
-    Eigen::Vector4d &u() override { return u_; }
+    Eigen::Vector4d& u() override {
+        return u_;
+    }
 
  protected:
     Eigen::Vector4d u_;
diff --git a/include/scrimmage/plugins/controller/SingleIntegratorControllerSimple/SingleIntegratorControllerSimple.h b/include/scrimmage/plugins/controller/SingleIntegratorControllerSimple/SingleIntegratorControllerSimple.h
index 71f3029d1e..f5a91c2f19 100644
--- a/include/scrimmage/plugins/controller/SingleIntegratorControllerSimple/SingleIntegratorControllerSimple.h
+++ b/include/scrimmage/plugins/controller/SingleIntegratorControllerSimple/SingleIntegratorControllerSimple.h
@@ -35,16 +35,16 @@
 
 #include <scrimmage/motion/Controller.h>
 
+#include <cmath>
 #include <map>
 #include <string>
-#include <cmath>
 
 namespace scrimmage {
 namespace controller {
 
 class SingleIntegratorControllerSimple : public Controller {
  public:
-    virtual void init(std::map<std::string, std::string> &params);
+    virtual void init(std::map<std::string, std::string>& params);
     virtual bool step(double t, double dt);
 
  protected:
@@ -60,6 +60,6 @@ class SingleIntegratorControllerSimple : public Controller {
     double vel_y_ = NAN;
     double vel_z_ = NAN;
 };
-} // namespace controller
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_CONTROLLER_SINGLEINTEGRATORCONTROLLERSIMPLE_SINGLEINTEGRATORCONTROLLERSIMPLE_H_
+}  // namespace controller
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_CONTROLLER_SINGLEINTEGRATORCONTROLLERSIMPLE_SINGLEINTEGRATORCONTROLLERSIMPLE_H_
diff --git a/include/scrimmage/plugins/controller/SingleIntegratorControllerWaypoint/SingleIntegratorControllerWaypoint.h b/include/scrimmage/plugins/controller/SingleIntegratorControllerWaypoint/SingleIntegratorControllerWaypoint.h
index 0719c9e097..bc92a85a4e 100644
--- a/include/scrimmage/plugins/controller/SingleIntegratorControllerWaypoint/SingleIntegratorControllerWaypoint.h
+++ b/include/scrimmage/plugins/controller/SingleIntegratorControllerWaypoint/SingleIntegratorControllerWaypoint.h
@@ -35,16 +35,16 @@
 
 #include <scrimmage/motion/Controller.h>
 
+#include <cmath>
 #include <map>
 #include <string>
-#include <cmath>
 
 namespace scrimmage {
 namespace controller {
 
 class SingleIntegratorControllerWaypoint : public Controller {
  public:
-    virtual void init(std::map<std::string, std::string> &params);
+    virtual void init(std::map<std::string, std::string>& params);
     virtual bool step(double t, double dt);
 
  protected:
@@ -59,6 +59,6 @@ class SingleIntegratorControllerWaypoint : public Controller {
     double gain_ = NAN;
 };
 
-} // namespace controller
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_CONTROLLER_SINGLEINTEGRATORCONTROLLERWAYPOINT_SINGLEINTEGRATORCONTROLLERWAYPOINT_H_
+}  // namespace controller
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_CONTROLLER_SINGLEINTEGRATORCONTROLLERWAYPOINT_SINGLEINTEGRATORCONTROLLERWAYPOINT_H_
diff --git a/include/scrimmage/plugins/controller/UUV6DOFLinearEnergy/UUV6DOFLinearEnergy.h b/include/scrimmage/plugins/controller/UUV6DOFLinearEnergy/UUV6DOFLinearEnergy.h
index 6199fca135..a0c584058a 100644
--- a/include/scrimmage/plugins/controller/UUV6DOFLinearEnergy/UUV6DOFLinearEnergy.h
+++ b/include/scrimmage/plugins/controller/UUV6DOFLinearEnergy/UUV6DOFLinearEnergy.h
@@ -43,7 +43,7 @@ namespace controller {
 
 class UUV6DOFLinearEnergy : public scrimmage::Controller {
  public:
-    void init(std::map<std::string, std::string> &params) override;
+    void init(std::map<std::string, std::string>& params) override;
     bool step(double t, double dt) override;
 
  protected:
@@ -61,6 +61,6 @@ class UUV6DOFLinearEnergy : public scrimmage::Controller {
     double energy_max_ = 5000;
     double energy_min_ = 0;
 };
-} // namespace controller
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_CONTROLLER_UUV6DOFLINEARENERGY_UUV6DOFLINEARENERGY_H_
+}  // namespace controller
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_CONTROLLER_UUV6DOFLINEARENERGY_UUV6DOFLINEARENERGY_H_
diff --git a/include/scrimmage/plugins/controller/UUV6DOFPIDController/UUV6DOFPIDController.h b/include/scrimmage/plugins/controller/UUV6DOFPIDController/UUV6DOFPIDController.h
index de9bf658ae..670c727b73 100644
--- a/include/scrimmage/plugins/controller/UUV6DOFPIDController/UUV6DOFPIDController.h
+++ b/include/scrimmage/plugins/controller/UUV6DOFPIDController/UUV6DOFPIDController.h
@@ -33,11 +33,10 @@
 #ifndef INCLUDE_SCRIMMAGE_PLUGINS_CONTROLLER_UUV6DOFPIDCONTROLLER_UUV6DOFPIDCONTROLLER_H_
 #define INCLUDE_SCRIMMAGE_PLUGINS_CONTROLLER_UUV6DOFPIDCONTROLLER_UUV6DOFPIDCONTROLLER_H_
 
-#include <scrimmage/motion/Controller.h>
 #include <scrimmage/common/PID.h>
+#include <scrimmage/motion/Controller.h>
 
 #include <Eigen/Dense>
-
 #include <map>
 #include <string>
 
@@ -47,7 +46,7 @@ namespace controller {
 class UUV6DOFPIDController : public scrimmage::Controller {
  public:
     UUV6DOFPIDController();
-    void init(std::map<std::string, std::string> &params) override;
+    void init(std::map<std::string, std::string>& params) override;
     bool step(double t, double dt) override;
 
  protected:
@@ -65,6 +64,6 @@ class UUV6DOFPIDController : public scrimmage::Controller {
     scrimmage::PID pitch_pid_;
     scrimmage::PID speed_pid_;
 };
-} // namespace controller
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_CONTROLLER_UUV6DOFPIDCONTROLLER_UUV6DOFPIDCONTROLLER_H_
+}  // namespace controller
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_CONTROLLER_UUV6DOFPIDCONTROLLER_UUV6DOFPIDCONTROLLER_H_
diff --git a/include/scrimmage/plugins/controller/UnicycleControllerPoint/UnicycleControllerPoint.h b/include/scrimmage/plugins/controller/UnicycleControllerPoint/UnicycleControllerPoint.h
index 1a540a2490..e783cffdb8 100644
--- a/include/scrimmage/plugins/controller/UnicycleControllerPoint/UnicycleControllerPoint.h
+++ b/include/scrimmage/plugins/controller/UnicycleControllerPoint/UnicycleControllerPoint.h
@@ -45,7 +45,7 @@ namespace controller {
  */
 class UnicycleControllerPoint : public Controller {
  public:
-    virtual void init(std::map<std::string, std::string> &params);
+    virtual void init(std::map<std::string, std::string>& params);
     virtual bool step(double t, double dt);
 
  protected:
@@ -61,6 +61,6 @@ class UnicycleControllerPoint : public Controller {
     uint8_t pitch_rate_idx_out_ = 0;
     uint8_t velocity_z_idx_out_ = 0;
 };
-} // namespace controller
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_CONTROLLER_UNICYCLECONTROLLERPOINT_UNICYCLECONTROLLERPOINT_H_
+}  // namespace controller
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_CONTROLLER_UNICYCLECONTROLLERPOINT_UNICYCLECONTROLLERPOINT_H_
diff --git a/include/scrimmage/plugins/controller/UnicyclePID/UnicyclePID.h b/include/scrimmage/plugins/controller/UnicyclePID/UnicyclePID.h
index ddf55fdf10..d40314aae4 100644
--- a/include/scrimmage/plugins/controller/UnicyclePID/UnicyclePID.h
+++ b/include/scrimmage/plugins/controller/UnicyclePID/UnicyclePID.h
@@ -33,12 +33,12 @@
 #ifndef INCLUDE_SCRIMMAGE_PLUGINS_CONTROLLER_UNICYCLEPID_UNICYCLEPID_H_
 #define INCLUDE_SCRIMMAGE_PLUGINS_CONTROLLER_UNICYCLEPID_UNICYCLEPID_H_
 
-#include <scrimmage/motion/Controller.h>
 #include <scrimmage/common/PID.h>
+#include <scrimmage/motion/Controller.h>
 
 #include <map>
-#include <string>
 #include <memory>
+#include <string>
 
 namespace scrimmage {
 namespace controller {
@@ -46,7 +46,7 @@ namespace controller {
 class UnicyclePID : public scrimmage::Controller {
  public:
     UnicyclePID();
-    void init(std::map<std::string, std::string> &params) override;
+    void init(std::map<std::string, std::string>& params) override;
     bool step(double t, double dt) override;
 
  protected:
@@ -71,6 +71,6 @@ class UnicyclePID : public scrimmage::Controller {
 
     bool use_accel_ = false;
 };
-} // namespace controller
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_CONTROLLER_UNICYCLEPID_UNICYCLEPID_H_
+}  // namespace controller
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_CONTROLLER_UNICYCLEPID_UNICYCLEPID_H_
diff --git a/include/scrimmage/plugins/interaction/Boundary/Boundary.h b/include/scrimmage/plugins/interaction/Boundary/Boundary.h
index 6f6702963c..0ee400c77d 100644
--- a/include/scrimmage/plugins/interaction/Boundary/Boundary.h
+++ b/include/scrimmage/plugins/interaction/Boundary/Boundary.h
@@ -33,15 +33,15 @@
 #ifndef INCLUDE_SCRIMMAGE_PLUGINS_INTERACTION_BOUNDARY_BOUNDARY_H_
 #define INCLUDE_SCRIMMAGE_PLUGINS_INTERACTION_BOUNDARY_BOUNDARY_H_
 
-#include <scrimmage/simcontrol/EntityInteraction.h>
 #include <scrimmage/entity/Entity.h>
 #include <scrimmage/plugins/interaction/Boundary/BoundaryBase.h>
 #include <scrimmage/proto/Shape.pb.h>
+#include <scrimmage/simcontrol/EntityInteraction.h>
 
-#include <map>
-#include <string>
 #include <list>
+#include <map>
 #include <memory>
+#include <string>
 
 namespace sc = scrimmage;
 
@@ -51,18 +51,17 @@ namespace interaction {
 class Boundary : public scrimmage::EntityInteraction {
  public:
     Boundary();
-    bool init(std::map<std::string, std::string> &mission_params,
-              std::map<std::string, std::string> &plugin_params) override;
-    bool step_entity_interaction(std::list<sc::EntityPtr> &ents,
-                                 double t, double dt) override;
+    bool init(std::map<std::string, std::string>& mission_params,
+              std::map<std::string, std::string>& plugin_params) override;
+    bool step_entity_interaction(std::list<sc::EntityPtr>& ents, double t, double dt) override;
 
-    static std::shared_ptr<BoundaryBase> make_boundary(const scrimmage_proto::Shape &shape);
+    static std::shared_ptr<BoundaryBase> make_boundary(const scrimmage_proto::Shape& shape);
 
  protected:
     sc::PublisherPtr pub_boundary_;
     bool boundary_published_ = false;
     std::shared_ptr<scrimmage_proto::Shape> boundary_shape_;
 };
-} // namespace interaction
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_INTERACTION_BOUNDARY_BOUNDARY_H_
+}  // namespace interaction
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_INTERACTION_BOUNDARY_BOUNDARY_H_
diff --git a/include/scrimmage/plugins/interaction/Boundary/BoundaryBase.h b/include/scrimmage/plugins/interaction/Boundary/BoundaryBase.h
index 530b5b7745..c9cb6f71a9 100644
--- a/include/scrimmage/plugins/interaction/Boundary/BoundaryBase.h
+++ b/include/scrimmage/plugins/interaction/Boundary/BoundaryBase.h
@@ -37,7 +37,6 @@
 #include <scrimmage/proto/Shape.pb.h>
 
 #include <Eigen/Dense>
-
 #include <list>
 #include <memory>
 #include <tuple>
@@ -51,18 +50,27 @@ namespace interaction {
 
 class BoundaryBase {
  public:
-    BoundaryBase()
-        : center_(0, 0, 0) {}
-    virtual ~BoundaryBase() {}
-    explicit BoundaryBase(const Eigen::Vector3d &center)
-        : center_(center) {}
+    BoundaryBase() : center_(0, 0, 0) {
+    }
+    virtual ~BoundaryBase() {
+    }
+    explicit BoundaryBase(const Eigen::Vector3d& center) : center_(center) {
+    }
     virtual bool contains(Eigen::Vector3d p) = 0;
-    const sc::ShapePtr shape() const { return shape_; }
-    void set_shape(const sc::ShapePtr &shape) { shape_ = shape; }
+    const sc::ShapePtr shape() const {
+        return shape_;
+    }
+    void set_shape(const sc::ShapePtr& shape) {
+        shape_ = shape;
+    }
     virtual void set_visual(int R, int G, int B, double opacity) = 0;
-    virtual Eigen::Vector3d center() { return center_; }
+    virtual Eigen::Vector3d center() {
+        return center_;
+    }
 
-    virtual const std::vector<std::tuple<double, double>> &extents() { return extents_; }
+    virtual const std::vector<std::tuple<double, double>>& extents() {
+        return extents_;
+    }
 
  protected:
     sc::ShapePtr shape_;
diff --git a/include/scrimmage/plugins/interaction/Boundary/Cuboid.h b/include/scrimmage/plugins/interaction/Boundary/Cuboid.h
index ec15f00a27..812dd5908d 100644
--- a/include/scrimmage/plugins/interaction/Boundary/Cuboid.h
+++ b/include/scrimmage/plugins/interaction/Boundary/Cuboid.h
@@ -38,7 +38,6 @@
 #include <scrimmage/proto/ProtoConversions.h>
 
 #include <Eigen/Dense>
-
 #include <iostream>
 #include <limits>
 #include <tuple>
@@ -54,12 +53,10 @@ namespace interaction {
 
 class Cuboid : public BoundaryBase {
  public:
-    Cuboid() {}
+    Cuboid() {
+    }
 
-    Cuboid(Eigen::Vector3d center,
-           double x_length,
-           double y_width,
-           double z_height,
+    Cuboid(Eigen::Vector3d center, double x_length, double y_width, double z_height,
            scrimmage::Quaternion quat) {
         double x = x_length / 2.0;
         double y = y_width / 2.0;
@@ -77,17 +74,15 @@ class Cuboid : public BoundaryBase {
 
         // TODO: Handle rotation
 
-        for (Eigen::Vector3d &p : points) {
+        for (Eigen::Vector3d& p : points) {
             p += center;
         }
         set_points(points);
     }
 
-    explicit Cuboid(const scrimmage_proto::Shape &shape)
-        : Cuboid(proto_2_vector3d(shape.cuboid().center()),
-                 shape.cuboid().x_length(),
-                 shape.cuboid().y_length(),
-                 shape.cuboid().z_length(),
+    explicit Cuboid(const scrimmage_proto::Shape& shape)
+        : Cuboid(proto_2_vector3d(shape.cuboid().center()), shape.cuboid().x_length(),
+                 shape.cuboid().y_length(), shape.cuboid().z_length(),
                  sc::proto_2_quat(shape.cuboid().quat())) {
         set_visual(shape.color().r(), shape.color().g(), shape.color().b(), shape.opacity());
     }
@@ -112,16 +107,18 @@ class Cuboid : public BoundaryBase {
         double v_dot_p = v.dot(p);
         double w_dot_p = w.dot(p);
 
-        if ((u_dot_P0 > u_dot_p) && (u_dot_p > u_dot_P1) && (v_dot_P0 > v_dot_p) &&
-            (v_dot_p > v_dot_P3) && (w_dot_P0 > w_dot_p) && (w_dot_p > w_dot_P4)) {
+        if ((u_dot_P0 > u_dot_p) && (u_dot_p > u_dot_P1) && (v_dot_P0 > v_dot_p)
+            && (v_dot_p > v_dot_P3) && (w_dot_P0 > w_dot_p) && (w_dot_p > w_dot_P4)) {
             return true;
         }
         return false;
     }
 
-    const std::vector<Eigen::Vector3d> &points() { return points_; }
+    const std::vector<Eigen::Vector3d>& points() {
+        return points_;
+    }
 
-    void set_points(std::vector<Eigen::Vector3d> &points) {
+    void set_points(std::vector<Eigen::Vector3d>& points) {
         points_ = points;
         compute_dots();
 
@@ -158,12 +155,7 @@ class Cuboid : public BoundaryBase {
         const int num_faces = 6;
         const int vert_per_face = 4;
         int vert_lookup[num_faces][vert_per_face] = {
-            {0, 1, 2, 3},
-            {0, 1, 5, 4},
-            {0, 3, 7, 4},
-            {6, 5, 4, 7},
-            {6, 2, 1, 5},
-            {6, 7, 3, 2},
+            {0, 1, 2, 3}, {0, 1, 5, 4}, {0, 3, 7, 4}, {6, 5, 4, 7}, {6, 2, 1, 5}, {6, 7, 3, 2},
         };
 
         for (int f = 0; f < num_faces; f++) {
@@ -173,7 +165,7 @@ class Cuboid : public BoundaryBase {
             sc::set(polygon->mutable_color(), R, G, B);
 
             for (int r = 0; r < vert_per_face; r++) {
-                sp::Vector3d *p = polygon->mutable_polygon()->add_point();
+                sp::Vector3d* p = polygon->mutable_polygon()->add_point();
                 sc::set(p, points_[vert_lookup[f][r]]);
             }
             // shapes_.push_back(polygon);
diff --git a/include/scrimmage/plugins/interaction/Boundary/Plane.h b/include/scrimmage/plugins/interaction/Boundary/Plane.h
index d7d5a00636..d46db04f03 100644
--- a/include/scrimmage/plugins/interaction/Boundary/Plane.h
+++ b/include/scrimmage/plugins/interaction/Boundary/Plane.h
@@ -38,7 +38,6 @@
 #include <scrimmage/proto/ProtoConversions.h>
 
 #include <Eigen/Dense>
-
 #include <iostream>
 #include <limits>
 #include <string>
@@ -55,14 +54,11 @@ namespace interaction {
 
 class Plane : public BoundaryBase {
  public:
-    Plane() {}
-
-    Plane(Eigen::Vector3d center,
-          double x_length,
-          double y_width,
-          scrimmage::Quaternion quat,
-          std::string texture,
-          bool diffuse_lighting) {
+    Plane() {
+    }
+
+    Plane(Eigen::Vector3d center, double x_length, double y_width, scrimmage::Quaternion quat,
+          std::string texture, bool diffuse_lighting) {
         double x = x_length / 2.0;
         double y = y_width / 2.0;
         double z = center(2);
@@ -80,19 +76,16 @@ class Plane : public BoundaryBase {
 
         // TODO: Handle rotation
 
-        for (Eigen::Vector3d &p : points) {
+        for (Eigen::Vector3d& p : points) {
             p += center;
         }
         set_points(points);
     }
 
-    explicit Plane(const scrimmage_proto::Shape &shape)
-        : Plane(proto_2_vector3d(shape.plane().center()),
-                shape.plane().x_length(),
-                shape.plane().y_length(),
-                proto_2_quat(shape.plane().quat()),
-                shape.plane().texture(),
-                shape.plane().diffuse_lighting()) {
+    explicit Plane(const scrimmage_proto::Shape& shape)
+        : Plane(proto_2_vector3d(shape.plane().center()), shape.plane().x_length(),
+                shape.plane().y_length(), proto_2_quat(shape.plane().quat()),
+                shape.plane().texture(), shape.plane().diffuse_lighting()) {
         set_visual(shape.color().r(), shape.color().g(), shape.color().b(), shape.opacity());
     }
 
@@ -120,16 +113,18 @@ class Plane : public BoundaryBase {
         double r_dot_p = r.dot(p - center_);
         double s_dot_p = s.dot(p - center_);
 
-        if ((u_dot_p > u_dot_P0) && (w_dot_p > w_dot_P0) && (v_dot_p > v_dot_P2) &&
-            (s_dot_p > s_dot_P1) && (r_dot_p > r_dot_P3)) {
+        if ((u_dot_p > u_dot_P0) && (w_dot_p > w_dot_P0) && (v_dot_p > v_dot_P2)
+            && (s_dot_p > s_dot_P1) && (r_dot_p > r_dot_P3)) {
             return true;
         }
         return false;
     }
 
-    const std::vector<Eigen::Vector3d> &points() { return points_; }
+    const std::vector<Eigen::Vector3d>& points() {
+        return points_;
+    }
 
-    void set_points(std::vector<Eigen::Vector3d> &points) {
+    void set_points(std::vector<Eigen::Vector3d>& points) {
         points_ = points;
         compute_dots();
 
@@ -177,7 +172,7 @@ class Plane : public BoundaryBase {
             sc::set(polygon->mutable_color(), R, G, B);
 
             for (int r = 0; r < vert_per_face; r++) {
-                sp::Vector3d *p = polygon->mutable_polygon()->add_point();
+                sp::Vector3d* p = polygon->mutable_polygon()->add_point();
                 sc::set(p, points_[vert_lookup[f][r]]);
             }
             // shapes_.push_back(polygon);
diff --git a/include/scrimmage/plugins/interaction/Boundary/Polyhedron.h b/include/scrimmage/plugins/interaction/Boundary/Polyhedron.h
index d2365a232e..a40d2d3daa 100644
--- a/include/scrimmage/plugins/interaction/Boundary/Polyhedron.h
+++ b/include/scrimmage/plugins/interaction/Boundary/Polyhedron.h
@@ -37,7 +37,6 @@
 #include <scrimmage/plugins/interaction/Boundary/BoundaryBase.h>
 
 #include <Eigen/Dense>
-
 #include <iostream>
 #include <limits>
 #include <tuple>
@@ -51,12 +50,10 @@ namespace interaction {
 
 class Polyhedron : public BoundaryBase {
  public:
-    Polyhedron() {}
+    Polyhedron() {
+    }
 
-    Polyhedron(Eigen::Vector3d center,
-               double x_length,
-               double y_width,
-               double z_height,
+    Polyhedron(Eigen::Vector3d center, double x_length, double y_width, double z_height,
                scrimmage::Quaternion quat) {
         double x = x_length / 2.0;
         double y = y_width / 2.0;
@@ -74,7 +71,7 @@ class Polyhedron : public BoundaryBase {
 
         // TODO: Handle rotation
 
-        for (Eigen::Vector3d &p : points) {
+        for (Eigen::Vector3d& p : points) {
             p += center;
         }
         set_points(points);
@@ -100,14 +97,14 @@ class Polyhedron : public BoundaryBase {
         double v_dot_p = v.dot(p);
         double w_dot_p = w.dot(p);
 
-        if ((u_dot_P0 > u_dot_p) && (u_dot_p > u_dot_P1) && (v_dot_P0 > v_dot_p) &&
-            (v_dot_p > v_dot_P3) && (w_dot_P0 > w_dot_p) && (w_dot_p > w_dot_P4)) {
+        if ((u_dot_P0 > u_dot_p) && (u_dot_p > u_dot_P1) && (v_dot_P0 > v_dot_p)
+            && (v_dot_p > v_dot_P3) && (w_dot_P0 > w_dot_p) && (w_dot_p > w_dot_P4)) {
             return true;
         }
         return false;
     }
 
-    void set_points(std::vector<Eigen::Vector3d> &points) {
+    void set_points(std::vector<Eigen::Vector3d>& points) {
         points_ = points;
         compute_dots();
 
@@ -144,12 +141,7 @@ class Polyhedron : public BoundaryBase {
         const int num_faces = 6;
         const int vert_per_face = 4;
         int vert_lookup[num_faces][vert_per_face] = {
-            {0, 1, 2, 3},
-            {0, 1, 5, 4},
-            {0, 3, 7, 4},
-            {6, 5, 4, 7},
-            {6, 2, 1, 5},
-            {6, 7, 3, 2},
+            {0, 1, 2, 3}, {0, 1, 5, 4}, {0, 3, 7, 4}, {6, 5, 4, 7}, {6, 2, 1, 5}, {6, 7, 3, 2},
         };
 
         for (int f = 0; f < num_faces; f++) {
@@ -159,14 +151,16 @@ class Polyhedron : public BoundaryBase {
             sc::set(polygon->mutable_color(), R, G, B);
 
             for (int r = 0; r < vert_per_face; r++) {
-                sp::Vector3d *p = polygon->mutable_polygon()->add_point();
+                sp::Vector3d* p = polygon->mutable_polygon()->add_point();
                 sc::set(p, points_[vert_lookup[f][r]]);
             }
             shapes_.push_back(polygon);
         }
     }
 
-    const std::vector<std::tuple<double, double>> &get_bounds() const { return bounds_; }
+    const std::vector<std::tuple<double, double>>& get_bounds() const {
+        return bounds_;
+    }
 
  protected:
     std::vector<Eigen::Vector3d> points_;
diff --git a/include/scrimmage/plugins/interaction/Boundary/Sphere.h b/include/scrimmage/plugins/interaction/Boundary/Sphere.h
index 7c86130952..213b08ab24 100644
--- a/include/scrimmage/plugins/interaction/Boundary/Sphere.h
+++ b/include/scrimmage/plugins/interaction/Boundary/Sphere.h
@@ -37,10 +37,9 @@
 #include <scrimmage/proto/ProtoConversions.h>
 
 #include <Eigen/Dense>
-
 #include <iostream>
-#include <vector>
 #include <tuple>
+#include <vector>
 
 using std::cout;
 using std::endl;
@@ -55,21 +54,30 @@ class Sphere : public BoundaryBase {
     Sphere() : BoundaryBase(Eigen::Vector3d(0, 0, 0)), radius_(1.0) {
     }
 
-    Sphere(const double &radius, const Eigen::Vector3d &center) :
-    BoundaryBase(center), radius_(radius) {}
+    Sphere(const double& radius, const Eigen::Vector3d& center)
+        : BoundaryBase(center), radius_(radius) {
+    }
 
-    explicit Sphere(const scrimmage_proto::Shape &shape) :
-    Sphere(shape.sphere().radius(), proto_2_vector3d(shape.sphere().center())) {
+    explicit Sphere(const scrimmage_proto::Shape& shape)
+        : Sphere(shape.sphere().radius(), proto_2_vector3d(shape.sphere().center())) {
     }
 
-    void set_radius(const double &radius) { radius_ = radius; }
-    void set_center(const Eigen::Vector3d &center) { center_ = center; }
+    void set_radius(const double& radius) {
+        radius_ = radius;
+    }
+    void set_center(const Eigen::Vector3d& center) {
+        center_ = center;
+    }
 
-    double radius() { return radius_; }
-    Eigen::Vector3d center() override { return center_; }
+    double radius() {
+        return radius_;
+    }
+    Eigen::Vector3d center() override {
+        return center_;
+    }
 
     bool contains(Eigen::Vector3d p) override {
-        return (p-center_).norm() < radius_;
+        return (p - center_).norm() < radius_;
     }
 
     void set_visual(int R, int G, int B, double opacity) override {
@@ -82,12 +90,12 @@ class Sphere : public BoundaryBase {
         shapes_.push_back(sphere);
     }
 
-    const std::vector<std::tuple<double, double>> &extents() override {
+    const std::vector<std::tuple<double, double>>& extents() override {
         // Find the smallest cube that covers the entire sphere
         extents_.clear();
         for (unsigned int i = 0; i < 3; i++) {
-            extents_.push_back(std::tuple<double, double>(center_(i)+radius_,
-                                                          center_(i)-radius_));
+            extents_.push_back(
+                std::tuple<double, double>(center_(i) + radius_, center_(i) - radius_));
         }
         return extents_;
     }
@@ -95,6 +103,6 @@ class Sphere : public BoundaryBase {
  protected:
     double radius_;
 };
-} // namespace interaction
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_INTERACTION_BOUNDARY_SPHERE_H_
+}  // namespace interaction
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_INTERACTION_BOUNDARY_SPHERE_H_
diff --git a/include/scrimmage/plugins/interaction/BulletCollision/BulletCollision.h b/include/scrimmage/plugins/interaction/BulletCollision/BulletCollision.h
index 939cee4b83..bb8826b83c 100644
--- a/include/scrimmage/plugins/interaction/BulletCollision/BulletCollision.h
+++ b/include/scrimmage/plugins/interaction/BulletCollision/BulletCollision.h
@@ -33,20 +33,19 @@
 #ifndef INCLUDE_SCRIMMAGE_PLUGINS_INTERACTION_BULLETCOLLISION_BULLETCOLLISION_H_
 #define INCLUDE_SCRIMMAGE_PLUGINS_INTERACTION_BULLETCOLLISION_BULLETCOLLISION_H_
 
-#include <scrimmage/simcontrol/EntityInteraction.h>
+#include <btBulletDynamicsCommon.h>
 #include <scrimmage/entity/Entity.h>
-#include <scrimmage/pubsub/Subscriber.h>
-#include <scrimmage/pubsub/Publisher.h>
 #include <scrimmage/plugins/sensor/RayTrace/RayTrace.h>
+#include <scrimmage/pubsub/Publisher.h>
+#include <scrimmage/pubsub/Subscriber.h>
+#include <scrimmage/simcontrol/EntityInteraction.h>
 
-#include <btBulletDynamicsCommon.h>
-
-#include <vector>
 #include <list>
 #include <map>
-#include <string>
 #include <memory>
+#include <string>
 #include <utility>
+#include <vector>
 
 namespace sc = scrimmage;
 class Interface;
@@ -58,24 +57,21 @@ class BulletCollision : public scrimmage::EntityInteraction {
  public:
     BulletCollision();
     ~BulletCollision();
-    bool init(std::map<std::string, std::string> &mission_params,
-                      std::map<std::string, std::string> &plugin_params) override;
+    bool init(std::map<std::string, std::string>& mission_params,
+              std::map<std::string, std::string>& plugin_params) override;
 
-    bool step_entity_interaction(std::list<sc::EntityPtr> &ents,
-                                         double t, double dt) override;
+    bool step_entity_interaction(std::list<sc::EntityPtr>& ents, double t, double dt) override;
 
-    bool collision_exists(std::list<sc::EntityPtr> &ents,
-                                  Eigen::Vector3d &p) override;
+    bool collision_exists(std::list<sc::EntityPtr>& ents, Eigen::Vector3d& p) override;
 
     /** For the service call */
-    bool get_ray_tracing(scrimmage::MessageBasePtr request,
-                         scrimmage::MessageBasePtr &response);
+    bool get_ray_tracing(scrimmage::MessageBasePtr request, scrimmage::MessageBasePtr& response);
 
  protected:
-    std::pair<bool, scrimmage::EntityPtr> get_entity(const int &id);
-    void remove_entity_object(const int &id);
-    void entity_collision(const int &id);
-    void remove_object(const int &id);
+    std::pair<bool, scrimmage::EntityPtr> get_entity(const int& id);
+    void remove_entity_object(const int& id);
+    void entity_collision(const int& id);
+    void remove_object(const int& id);
 
     btCollisionConfiguration* bt_collision_configuration;
     btCollisionDispatcher* bt_dispatcher;
diff --git a/include/scrimmage/plugins/interaction/CaptureInBoundaryInteraction/CaptureInBoundaryInteraction.h b/include/scrimmage/plugins/interaction/CaptureInBoundaryInteraction/CaptureInBoundaryInteraction.h
index a562af2ac9..84edc35db1 100644
--- a/include/scrimmage/plugins/interaction/CaptureInBoundaryInteraction/CaptureInBoundaryInteraction.h
+++ b/include/scrimmage/plugins/interaction/CaptureInBoundaryInteraction/CaptureInBoundaryInteraction.h
@@ -33,16 +33,15 @@
 #ifndef INCLUDE_SCRIMMAGE_PLUGINS_INTERACTION_CAPTUREINBOUNDARYINTERACTION_CAPTUREINBOUNDARYINTERACTION_H_
 #define INCLUDE_SCRIMMAGE_PLUGINS_INTERACTION_CAPTUREINBOUNDARYINTERACTION_CAPTUREINBOUNDARYINTERACTION_H_
 
-#include <scrimmage/simcontrol/EntityInteraction.h>
 #include <scrimmage/entity/Entity.h>
-
-#include <scrimmage/proto/Shape.pb.h>
 #include <scrimmage/plugins/interaction/Boundary/BoundaryBase.h>
+#include <scrimmage/proto/Shape.pb.h>
+#include <scrimmage/simcontrol/EntityInteraction.h>
 
-#include <map>
 #include <list>
-#include <string>
+#include <map>
 #include <memory>
+#include <string>
 #include <unordered_set>
 
 namespace sc = scrimmage;
@@ -54,10 +53,10 @@ namespace interaction {
 class CaptureInBoundaryInteraction : public scrimmage::EntityInteraction {
  public:
     CaptureInBoundaryInteraction();
-    bool init(std::map<std::string, std::string> &mission_params,
-              std::map<std::string, std::string> &plugin_params) override;
-    bool step_entity_interaction(std::list<sc::EntityPtr> &ents,
-                                 double t, double dt) override;
+    bool init(std::map<std::string, std::string>& mission_params,
+              std::map<std::string, std::string>& plugin_params) override;
+    bool step_entity_interaction(std::list<sc::EntityPtr>& ents, double t, double dt) override;
+
  protected:
     std::shared_ptr<sci::BoundaryBase> boundary_;
     scrimmage_proto::Shape boundary_shape_;
@@ -68,8 +67,9 @@ class CaptureInBoundaryInteraction : public scrimmage::EntityInteraction {
 
     scrimmage::PublisherPtr non_team_capture_pub_;
     std::unordered_set<int> already_captured_;
+
  private:
 };
-} // namespace interaction
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_INTERACTION_CAPTUREINBOUNDARYINTERACTION_CAPTUREINBOUNDARYINTERACTION_H_
+}  // namespace interaction
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_INTERACTION_CAPTUREINBOUNDARYINTERACTION_CAPTUREINBOUNDARYINTERACTION_H_
diff --git a/include/scrimmage/plugins/interaction/EnforceBoundaryInteraction/EnforceBoundaryInteraction.h b/include/scrimmage/plugins/interaction/EnforceBoundaryInteraction/EnforceBoundaryInteraction.h
index 12a3efbbf3..0162a8b2c1 100644
--- a/include/scrimmage/plugins/interaction/EnforceBoundaryInteraction/EnforceBoundaryInteraction.h
+++ b/include/scrimmage/plugins/interaction/EnforceBoundaryInteraction/EnforceBoundaryInteraction.h
@@ -33,17 +33,16 @@
 #ifndef INCLUDE_SCRIMMAGE_PLUGINS_INTERACTION_ENFORCEBOUNDARYINTERACTION_ENFORCEBOUNDARYINTERACTION_H_
 #define INCLUDE_SCRIMMAGE_PLUGINS_INTERACTION_ENFORCEBOUNDARYINTERACTION_ENFORCEBOUNDARYINTERACTION_H_
 
-#include <scrimmage/simcontrol/EntityInteraction.h>
 #include <scrimmage/entity/Entity.h>
-
-#include <scrimmage/proto/Shape.pb.h>
 #include <scrimmage/plugins/interaction/Boundary/BoundaryBase.h>
+#include <scrimmage/proto/Shape.pb.h>
+#include <scrimmage/simcontrol/EntityInteraction.h>
 
-#include <map>
 #include <list>
-#include <string>
-#include <set>
+#include <map>
 #include <memory>
+#include <set>
+#include <string>
 
 namespace sc = scrimmage;
 namespace sci = scrimmage::interaction;
@@ -54,15 +53,16 @@ namespace interaction {
 class EnforceBoundaryInteraction : public scrimmage::EntityInteraction {
  public:
     EnforceBoundaryInteraction();
-    bool init(std::map<std::string, std::string> &mission_params,
-              std::map<std::string, std::string> &plugin_params) override;
-    bool step_entity_interaction(std::list<sc::EntityPtr> &ents,
-                                 double t, double dt) override;
+    bool init(std::map<std::string, std::string>& mission_params,
+              std::map<std::string, std::string>& plugin_params) override;
+    bool step_entity_interaction(std::list<sc::EntityPtr>& ents, double t, double dt) override;
+
  protected:
     std::list<std::shared_ptr<sci::BoundaryBase>> boundaries_;
     std::set<int> active_boundary_ids_;
+
  private:
 };
-} // namespace interaction
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_INTERACTION_ENFORCEBOUNDARYINTERACTION_ENFORCEBOUNDARYINTERACTION_H_
+}  // namespace interaction
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_INTERACTION_ENFORCEBOUNDARYINTERACTION_ENFORCEBOUNDARYINTERACTION_H_
diff --git a/include/scrimmage/plugins/interaction/ExternalForceField/ExternalForceField.h b/include/scrimmage/plugins/interaction/ExternalForceField/ExternalForceField.h
index 19e7393363..80a686164a 100644
--- a/include/scrimmage/plugins/interaction/ExternalForceField/ExternalForceField.h
+++ b/include/scrimmage/plugins/interaction/ExternalForceField/ExternalForceField.h
@@ -33,30 +33,27 @@
 #ifndef INCLUDE_SCRIMMAGE_PLUGINS_INTERACTION_EXTERNALFORCEFIELD_EXTERNALFORCEFIELD_H_
 #define INCLUDE_SCRIMMAGE_PLUGINS_INTERACTION_EXTERNALFORCEFIELD_EXTERNALFORCEFIELD_H_
 
-#include <scrimmage/simcontrol/EntityInteraction.h>
 #include <scrimmage/entity/Entity.h>
+#include <scrimmage/simcontrol/EntityInteraction.h>
 
-#include <map>
 #include <list>
+#include <map>
+#include <memory>
+#include <random>
 #include <string>
 #include <vector>
-#include <random>
-#include <memory>
 
 namespace scrimmage {
 namespace interaction {
 
 class ExternalForceField : public scrimmage::EntityInteraction {
  public:
-    enum ForceType {
-        Constant,
-        Variable
-    };
+    enum ForceType { Constant, Variable };
     ExternalForceField();
-    bool init(std::map<std::string, std::string> &mission_params,
-              std::map<std::string, std::string> &plugin_params) override;
-    bool step_entity_interaction(std::list<scrimmage::EntityPtr> &ents,
-                                 double t, double dt) override;
+    bool init(std::map<std::string, std::string>& mission_params,
+              std::map<std::string, std::string>& plugin_params) override;
+    bool step_entity_interaction(std::list<scrimmage::EntityPtr>& ents, double t,
+                                 double dt) override;
 
  protected:
     PublisherPtr pub_;
@@ -83,6 +80,6 @@ class ExternalForceField : public scrimmage::EntityInteraction {
 
  private:
 };
-} // namespace interaction
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_INTERACTION_EXTERNALFORCEFIELD_EXTERNALFORCEFIELD_H_
+}  // namespace interaction
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_INTERACTION_EXTERNALFORCEFIELD_EXTERNALFORCEFIELD_H_
diff --git a/include/scrimmage/plugins/interaction/FlagCaptureInteraction/FlagCaptureInteraction.h b/include/scrimmage/plugins/interaction/FlagCaptureInteraction/FlagCaptureInteraction.h
index f572bd0f9b..1e2b2dedcb 100644
--- a/include/scrimmage/plugins/interaction/FlagCaptureInteraction/FlagCaptureInteraction.h
+++ b/include/scrimmage/plugins/interaction/FlagCaptureInteraction/FlagCaptureInteraction.h
@@ -33,17 +33,16 @@
 #ifndef INCLUDE_SCRIMMAGE_PLUGINS_INTERACTION_FLAGCAPTUREINTERACTION_FLAGCAPTUREINTERACTION_H_
 #define INCLUDE_SCRIMMAGE_PLUGINS_INTERACTION_FLAGCAPTUREINTERACTION_FLAGCAPTUREINTERACTION_H_
 
-#include <scrimmage/simcontrol/EntityInteraction.h>
-#include <scrimmage/entity/Entity.h>
 #include <scrimmage/common/ID.h>
-
+#include <scrimmage/entity/Entity.h>
 #include <scrimmage/plugins/interaction/Boundary/BoundaryBase.h>
 #include <scrimmage/proto/Shape.pb.h>
+#include <scrimmage/simcontrol/EntityInteraction.h>
 
-#include <map>
 #include <list>
-#include <string>
+#include <map>
 #include <memory>
+#include <string>
 
 namespace sc = scrimmage;
 namespace sci = scrimmage::interaction;
@@ -54,10 +53,9 @@ namespace interaction {
 class FlagCaptureInteraction : public scrimmage::EntityInteraction {
  public:
     FlagCaptureInteraction();
-    bool init(std::map<std::string, std::string> &mission_params,
-              std::map<std::string, std::string> &plugin_params) override;
-    bool step_entity_interaction(std::list<sc::EntityPtr> &ents,
-                                 double t, double dt) override;
+    bool init(std::map<std::string, std::string>& mission_params,
+              std::map<std::string, std::string>& plugin_params) override;
+    bool step_entity_interaction(std::list<sc::EntityPtr>& ents, double t, double dt) override;
 
  protected:
     int flag_boundary_id_ = 0;
@@ -78,6 +76,6 @@ class FlagCaptureInteraction : public scrimmage::EntityInteraction {
 
  private:
 };
-} // namespace interaction
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_INTERACTION_FLAGCAPTUREINTERACTION_FLAGCAPTUREINTERACTION_H_
+}  // namespace interaction
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_INTERACTION_FLAGCAPTUREINTERACTION_FLAGCAPTUREINTERACTION_H_
diff --git a/include/scrimmage/plugins/interaction/GRPCCommandString/GRPCCommandString.h b/include/scrimmage/plugins/interaction/GRPCCommandString/GRPCCommandString.h
index dc219173c1..795b0947b7 100644
--- a/include/scrimmage/plugins/interaction/GRPCCommandString/GRPCCommandString.h
+++ b/include/scrimmage/plugins/interaction/GRPCCommandString/GRPCCommandString.h
@@ -33,16 +33,15 @@
 #ifndef INCLUDE_SCRIMMAGE_PLUGINS_INTERACTION_GRPCCOMMANDSTRING_GRPCCOMMANDSTRING_H_
 #define INCLUDE_SCRIMMAGE_PLUGINS_INTERACTION_GRPCCOMMANDSTRING_GRPCCOMMANDSTRING_H_
 
-#include <scrimmage/simcontrol/EntityInteraction.h>
-
 #include <scrimmage/msgs/Command.pb.h>
+#include <scrimmage/simcontrol/EntityInteraction.h>
 
-#include <map>
 #include <list>
-#include <string>
+#include <map>
+#include <mutex>  // NOLINT
 #include <queue>
-#include <thread> // NOLINT
-#include <mutex> // NOLINT
+#include <string>
+#include <thread>  // NOLINT
 
 namespace sc = scrimmage;
 
@@ -52,13 +51,12 @@ namespace interaction {
 class GRPCCommandString : public scrimmage::EntityInteraction {
  public:
     GRPCCommandString();
-    bool init(std::map<std::string, std::string> &mission_params,
-              std::map<std::string, std::string> &plugin_params) override;
-    bool step_entity_interaction(std::list<sc::EntityPtr> &ents,
-                                 double t, double dt) override;
+    bool init(std::map<std::string, std::string>& mission_params,
+              std::map<std::string, std::string>& plugin_params) override;
+    bool step_entity_interaction(std::list<sc::EntityPtr>& ents, double t, double dt) override;
     void run_server();
 
-    void push_msg(const scrimmage_msgs::CommandString &msg);
+    void push_msg(const scrimmage_msgs::CommandString& msg);
 
  protected:
     std::string ip_ = "localhost";
@@ -72,6 +70,6 @@ class GRPCCommandString : public scrimmage::EntityInteraction {
 
  private:
 };
-} // namespace interaction
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_INTERACTION_GRPCCOMMANDSTRING_GRPCCOMMANDSTRING_H_
+}  // namespace interaction
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_INTERACTION_GRPCCOMMANDSTRING_GRPCCOMMANDSTRING_H_
diff --git a/include/scrimmage/plugins/interaction/GRPCCommandString/ScrimmageMsgServiceImpl.h b/include/scrimmage/plugins/interaction/GRPCCommandString/ScrimmageMsgServiceImpl.h
index e7ea787d80..1516d7cd18 100644
--- a/include/scrimmage/plugins/interaction/GRPCCommandString/ScrimmageMsgServiceImpl.h
+++ b/include/scrimmage/plugins/interaction/GRPCCommandString/ScrimmageMsgServiceImpl.h
@@ -34,11 +34,11 @@
 #define INCLUDE_SCRIMMAGE_PLUGINS_INTERACTION_GRPCCOMMANDSTRING_SCRIMMAGEMSGSERVICEIMPL_H_
 
 #include <scrimmage/entity/EntityPlugin.h>
-#include <scrimmage/plugins/interaction/GRPCCommandString/GRPCCommandString.h>
 #include <scrimmage/msgs/Command.grpc.pb.h>
+#include <scrimmage/plugins/interaction/GRPCCommandString/GRPCCommandString.h>
 
-#include <queue>
 #include <memory>
+#include <queue>
 
 namespace scrimmage {
 class ScrimmageMsgServiceImpl final : public scrimmage_msgs::ScrimmageMsgService::Service {
@@ -47,9 +47,10 @@ class ScrimmageMsgServiceImpl final : public scrimmage_msgs::ScrimmageMsgService
     grpc::Status SendCommandString(grpc::ServerContext* context,
                                    const scrimmage_msgs::CommandString* cmd,
                                    scrimmage_msgs::CommandAck* reply) override;
+
  protected:
     std::shared_ptr<interaction::GRPCCommandString> plugin_;
 };
-} // namespace scrimmage
+}  // namespace scrimmage
 
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_INTERACTION_GRPCCOMMANDSTRING_SCRIMMAGEMSGSERVICEIMPL_H_
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_INTERACTION_GRPCCOMMANDSTRING_SCRIMMAGEMSGSERVICEIMPL_H_
diff --git a/include/scrimmage/plugins/interaction/GraphInteraction/GraphInteraction.h b/include/scrimmage/plugins/interaction/GraphInteraction/GraphInteraction.h
index 36a8a80b21..305bf01639 100644
--- a/include/scrimmage/plugins/interaction/GraphInteraction/GraphInteraction.h
+++ b/include/scrimmage/plugins/interaction/GraphInteraction/GraphInteraction.h
@@ -35,12 +35,11 @@
 
 #include <scrimmage/simcontrol/EntityInteraction.h>
 
-#include <map>
+#include <boost/graph/adjacency_list.hpp>
 #include <list>
-#include <string>
+#include <map>
 #include <memory>
-
-#include <boost/graph/adjacency_list.hpp>
+#include <string>
 
 namespace scrimmage {
 
@@ -55,10 +54,10 @@ namespace interaction {
 class GraphInteraction : public scrimmage::EntityInteraction {
  public:
     GraphInteraction();
-    bool init(std::map<std::string, std::string> &mission_params,
-                std::map<std::string, std::string> &plugin_params) override;
-    bool step_entity_interaction(std::list<scrimmage::EntityPtr> &ents,
-                                    double t, double dt) override;
+    bool init(std::map<std::string, std::string>& mission_params,
+              std::map<std::string, std::string>& plugin_params) override;
+    bool step_entity_interaction(std::list<scrimmage::EntityPtr>& ents, double t,
+                                 double dt) override;
 
  protected:
     struct GraphData {
@@ -79,11 +78,10 @@ class GraphInteraction : public scrimmage::EntityInteraction {
         std::string name;
     };
 
-    typedef boost::adjacency_list<
-        boost::vecS,  // edge storage
-        boost::vecS,   // vertex storage
-        boost::directedS,
-        VertexProperties, EdgeProperties> Graph;
+    typedef boost::adjacency_list<boost::vecS,  // edge storage
+                                  boost::vecS,  // vertex storage
+                                  boost::directedS, VertexProperties, EdgeProperties>
+        Graph;
     Graph g_;
 
  private:
@@ -93,4 +91,4 @@ class GraphInteraction : public scrimmage::EntityInteraction {
 };
 }  // namespace interaction
 }  // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_INTERACTION_GRAPHINTERACTION_GRAPHINTERACTION_H_
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_INTERACTION_GRAPHINTERACTION_GRAPHINTERACTION_H_
diff --git a/include/scrimmage/plugins/interaction/GraphInteraction/GraphUtils.h b/include/scrimmage/plugins/interaction/GraphInteraction/GraphUtils.h
index 33cf6a9156..d73227b46b 100644
--- a/include/scrimmage/plugins/interaction/GraphInteraction/GraphUtils.h
+++ b/include/scrimmage/plugins/interaction/GraphInteraction/GraphUtils.h
@@ -33,25 +33,24 @@
 #ifndef INCLUDE_SCRIMMAGE_PLUGINS_INTERACTION_GRAPHINTERACTION_GRAPHUTILS_H_
 #define INCLUDE_SCRIMMAGE_PLUGINS_INTERACTION_GRAPHINTERACTION_GRAPHUTILS_H_
 
+#include <boost/functional/hash.hpp>
 #include <memory>
-#include <unordered_map>
-#include <vector>
 #include <tuple>
+#include <unordered_map>
 #include <utility>
-
-#include <boost/functional/hash.hpp>
+#include <vector>
 
 namespace scrimmage_msgs {
 class Graph;
-} // namespace scrimmage_msgs
+}  // namespace scrimmage_msgs
 
 namespace scrimmage_proto {
 class Shape;
-} // namespace scrimmage_proto
+}  // namespace scrimmage_proto
 
 namespace scrimmage_proto {
 class Vector3d;
-} // namespace scrimmage_proto
+}  // namespace scrimmage_proto
 
 namespace scrimmage {
 
@@ -60,22 +59,21 @@ using EntityPluginPtr = std::shared_ptr<EntityPlugin>;
 
 namespace interaction {
 
-enum class DrawNodeLabels {NO, YES};
+enum class DrawNodeLabels { NO, YES };
 using NodePair = std::pair<uint64_t, uint64_t>;
 using NodePairColorMap = std::unordered_map<NodePair, std::vector<int>, boost::hash<NodePair>>;
-using NodePairShapeMap = std::unordered_map<NodePair, std::shared_ptr<scrimmage_proto::Shape>, boost::hash<NodePair>>;
+using NodePairShapeMap =
+    std::unordered_map<NodePair, std::shared_ptr<scrimmage_proto::Shape>, boost::hash<NodePair>>;
 using NodeShapeMap = std::unordered_map<uint64_t, std::shared_ptr<scrimmage_proto::Shape>>;
 
-std::tuple<std::shared_ptr<scrimmage_proto::Shape>, NodePairShapeMap, NodeShapeMap>
-draw_graph(
-    scrimmage_msgs::Graph &graph,
-    const std::unordered_map<uint64_t, scrimmage_proto::Vector3d> &node_idx_to_pos,
-    DrawNodeLabels draw_node_labels,
-    EntityPluginPtr plugin);
+std::tuple<std::shared_ptr<scrimmage_proto::Shape>, NodePairShapeMap, NodeShapeMap> draw_graph(
+    scrimmage_msgs::Graph& graph,
+    const std::unordered_map<uint64_t, scrimmage_proto::Vector3d>& node_idx_to_pos,
+    DrawNodeLabels draw_node_labels, EntityPluginPtr plugin);
 
 std::unordered_map<uint64_t, scrimmage_proto::Vector3d> nodes_idxs_to_pos_map(
-    const scrimmage_msgs::Graph &graph);
-} // namespace interaction
-} // namespace scrimmage
+    const scrimmage_msgs::Graph& graph);
+}  // namespace interaction
+}  // namespace scrimmage
 
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_INTERACTION_GRAPHINTERACTION_GRAPHUTILS_H_
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_INTERACTION_GRAPHINTERACTION_GRAPHUTILS_H_
diff --git a/include/scrimmage/plugins/interaction/GroundCollision/GroundCollision.h b/include/scrimmage/plugins/interaction/GroundCollision/GroundCollision.h
index a82e6cd3db..dadd5eb34c 100644
--- a/include/scrimmage/plugins/interaction/GroundCollision/GroundCollision.h
+++ b/include/scrimmage/plugins/interaction/GroundCollision/GroundCollision.h
@@ -33,8 +33,8 @@
 #ifndef INCLUDE_SCRIMMAGE_PLUGINS_INTERACTION_GROUNDCOLLISION_GROUNDCOLLISION_H_
 #define INCLUDE_SCRIMMAGE_PLUGINS_INTERACTION_GROUNDCOLLISION_GROUNDCOLLISION_H_
 
-#include <scrimmage/simcontrol/EntityInteraction.h>
 #include <scrimmage/pubsub/Publisher.h>
+#include <scrimmage/simcontrol/EntityInteraction.h>
 
 #include <list>
 #include <map>
@@ -46,14 +46,13 @@ class GroundCollision : public scrimmage::EntityInteraction {
  public:
     GroundCollision();
 
-    bool init(std::map<std::string, std::string> &mission_params,
-                      std::map<std::string, std::string> &plugin_params) override;
+    bool init(std::map<std::string, std::string>& mission_params,
+              std::map<std::string, std::string>& plugin_params) override;
 
-    bool step_entity_interaction(
-        std::list<scrimmage::EntityPtr> &ents, double t, double dt) override;
+    bool step_entity_interaction(std::list<scrimmage::EntityPtr>& ents, double t,
+                                 double dt) override;
 
-    bool collision_exists(
-        std::list<scrimmage::EntityPtr> &ents, Eigen::Vector3d &p) override;
+    bool collision_exists(std::list<scrimmage::EntityPtr>& ents, Eigen::Vector3d& p) override;
 
  protected:
     double ground_collision_z_;
@@ -62,6 +61,6 @@ class GroundCollision : public scrimmage::EntityInteraction {
     bool enable_startup_collisions_;
     std::string team_;
 };
-} // namespace interaction
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_INTERACTION_GROUNDCOLLISION_GROUNDCOLLISION_H_
+}  // namespace interaction
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_INTERACTION_GROUNDCOLLISION_GROUNDCOLLISION_H_
diff --git a/include/scrimmage/plugins/interaction/MapGen2D/Map2DInfo.h b/include/scrimmage/plugins/interaction/MapGen2D/Map2DInfo.h
index 4e88f675e9..6bf1dafcd8 100644
--- a/include/scrimmage/plugins/interaction/MapGen2D/Map2DInfo.h
+++ b/include/scrimmage/plugins/interaction/MapGen2D/Map2DInfo.h
@@ -34,7 +34,6 @@
 #define INCLUDE_SCRIMMAGE_PLUGINS_INTERACTION_MAPGEN2D_MAP2DINFO_H_
 
 #include <Eigen/Dense>
-
 #include <opencv2/core/core.hpp>
 
 namespace scrimmage {
diff --git a/include/scrimmage/plugins/interaction/MapGen2D/MapGen2D.h b/include/scrimmage/plugins/interaction/MapGen2D/MapGen2D.h
index e9a9bad0ef..376fe68058 100644
--- a/include/scrimmage/plugins/interaction/MapGen2D/MapGen2D.h
+++ b/include/scrimmage/plugins/interaction/MapGen2D/MapGen2D.h
@@ -33,34 +33,33 @@
 #ifndef INCLUDE_SCRIMMAGE_PLUGINS_INTERACTION_MAPGEN2D_MAPGEN2D_H_
 #define INCLUDE_SCRIMMAGE_PLUGINS_INTERACTION_MAPGEN2D_MAPGEN2D_H_
 
-#include <scrimmage/pubsub/Publisher.h>
-#include <scrimmage/simcontrol/EntityInteraction.h>
 #include <scrimmage/entity/Entity.h>
 #include <scrimmage/proto/Shape.pb.h>
+#include <scrimmage/pubsub/Publisher.h>
+#include <scrimmage/simcontrol/EntityInteraction.h>
 
 #include <list>
 #include <map>
-#include <string>
 #include <memory>
-
 #include <opencv2/core/core.hpp>
+#include <string>
 
 namespace scrimmage {
 namespace interaction {
 class MapGen2D : public scrimmage::EntityInteraction {
  public:
-    bool init(std::map<std::string, std::string> &mission_params,
-              std::map<std::string, std::string> &plugin_params) override;
-    bool step_entity_interaction(
-        std::list<scrimmage::EntityPtr> &ents, double t, double dt) override;
+    bool init(std::map<std::string, std::string>& mission_params,
+              std::map<std::string, std::string>& plugin_params) override;
+    bool step_entity_interaction(std::list<scrimmage::EntityPtr>& ents, double t,
+                                 double dt) override;
 
  protected:
-    std::shared_ptr<scrimmage_proto::Shape> connect_points(
-        Eigen::Vector3d &p, Eigen::Vector3d &prev_p);
+    std::shared_ptr<scrimmage_proto::Shape> connect_points(Eigen::Vector3d& p,
+                                                           Eigen::Vector3d& prev_p);
 
-    std::list<cv::Rect> find_rectangles(cv::Mat &img, int threshold);
+    std::list<cv::Rect> find_rectangles(cv::Mat& img, int threshold);
 
-    Eigen::Vector3d img_xy_to_xyz(int x, int y, cv::Mat &img);
+    Eigen::Vector3d img_xy_to_xyz(int x, int y, cv::Mat& img);
 
     bool show_map_debug_;
     bool enable_map_boundary_;
@@ -81,6 +80,6 @@ class MapGen2D : public scrimmage::EntityInteraction {
 
     cv::Mat map_img_;
 };
-} // namespace interaction
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_INTERACTION_MAPGEN2D_MAPGEN2D_H_
+}  // namespace interaction
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_INTERACTION_MAPGEN2D_MAPGEN2D_H_
diff --git a/include/scrimmage/plugins/interaction/ROSClockServer/ROSClockServer.h b/include/scrimmage/plugins/interaction/ROSClockServer/ROSClockServer.h
index 2390c0ce4e..d5d4b5e12c 100644
--- a/include/scrimmage/plugins/interaction/ROSClockServer/ROSClockServer.h
+++ b/include/scrimmage/plugins/interaction/ROSClockServer/ROSClockServer.h
@@ -33,18 +33,17 @@
 #ifndef INCLUDE_SCRIMMAGE_PLUGINS_INTERACTION_ROSCLOCKSERVER_ROSCLOCKSERVER_H_
 #define INCLUDE_SCRIMMAGE_PLUGINS_INTERACTION_ROSCLOCKSERVER_ROSCLOCKSERVER_H_
 
-#include <scrimmage/simcontrol/EntityInteraction.h>
-#include <scrimmage/entity/Entity.h>
-
 #include <ros/ros.h>
+#include <scrimmage/entity/Entity.h>
+#include <scrimmage/simcontrol/EntityInteraction.h>
 
-#include <map>
+#include <chrono>  // NOLINT
 #include <list>
-#include <string>
-#include <chrono> // NOLINT
+#include <map>
 #include <memory>
-#include <thread>
 #include <mutex>
+#include <string>
+#include <thread>
 
 namespace scrimmage {
 namespace interaction {
@@ -52,11 +51,12 @@ namespace interaction {
 class ROSClockServer : public scrimmage::EntityInteraction {
  public:
     ROSClockServer();
-    bool init(std::map<std::string, std::string> &mission_params,
-              std::map<std::string, std::string> &plugin_params) override;
-    bool step_entity_interaction(std::list<scrimmage::EntityPtr> &ents,
-                                 double t, double dt) override;
+    bool init(std::map<std::string, std::string>& mission_params,
+              std::map<std::string, std::string>& plugin_params) override;
+    bool step_entity_interaction(std::list<scrimmage::EntityPtr>& ents, double t,
+                                 double dt) override;
     void close(double t) override;
+
  protected:
     void publish_clock_msg(const double& t);
 
@@ -73,9 +73,9 @@ class ROSClockServer : public scrimmage::EntityInteraction {
     bool running_ = false;
     std::mutex pub_mutex_;
 
-    std::chrono::time_point<std::chrono::system_clock,
-                            std::chrono::duration<double>> sim_start_time_;
+    std::chrono::time_point<std::chrono::system_clock, std::chrono::duration<double>>
+        sim_start_time_;
 };
-} // namespace interaction
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_INTERACTION_ROSCLOCKSERVER_ROSCLOCKSERVER_H_
+}  // namespace interaction
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_INTERACTION_ROSCLOCKSERVER_ROSCLOCKSERVER_H_
diff --git a/include/scrimmage/plugins/interaction/ROSShapeViz/ROSShapeViz.h b/include/scrimmage/plugins/interaction/ROSShapeViz/ROSShapeViz.h
index 6055c5cb6a..78a9f636ef 100644
--- a/include/scrimmage/plugins/interaction/ROSShapeViz/ROSShapeViz.h
+++ b/include/scrimmage/plugins/interaction/ROSShapeViz/ROSShapeViz.h
@@ -33,16 +33,15 @@
 #ifndef INCLUDE_SCRIMMAGE_PLUGINS_INTERACTION_ROSSHAPEVIZ_ROSSHAPEVIZ_H_
 #define INCLUDE_SCRIMMAGE_PLUGINS_INTERACTION_ROSSHAPEVIZ_ROSSHAPEVIZ_H_
 
-#include <scrimmage/simcontrol/EntityInteraction.h>
-#include <scrimmage/entity/Entity.h>
-
 #include <ros/ros.h>
+#include <scrimmage/entity/Entity.h>
+#include <scrimmage/simcontrol/EntityInteraction.h>
 #include <visualization_msgs/Marker.h>
 
-#include <map>
 #include <list>
-#include <string>
+#include <map>
 #include <memory>
+#include <string>
 
 namespace scrimmage {
 namespace interaction {
@@ -50,16 +49,17 @@ namespace interaction {
 class ROSShapeViz : public scrimmage::EntityInteraction {
  public:
     ROSShapeViz();
-    bool init(std::map<std::string, std::string> &mission_params,
-              std::map<std::string, std::string> &plugin_params) override;
-    bool step_entity_interaction(std::list<scrimmage::EntityPtr> &ents,
-                                 double t, double dt) override;
+    bool init(std::map<std::string, std::string>& mission_params,
+              std::map<std::string, std::string>& plugin_params) override;
+    bool step_entity_interaction(std::list<scrimmage::EntityPtr>& ents, double t,
+                                 double dt) override;
+
  protected:
  private:
     void shapes_cb(const visualization_msgs::Marker::ConstPtr& msg);
     std::shared_ptr<ros::NodeHandle> nh_;
     ros::Subscriber sub_shapes_;
 };
-} // namespace interaction
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_INTERACTION_ROSSHAPEVIZ_ROSSHAPEVIZ_H_
+}  // namespace interaction
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_INTERACTION_ROSSHAPEVIZ_ROSSHAPEVIZ_H_
diff --git a/include/scrimmage/plugins/interaction/RandomAttrit/RandomAttrit.h b/include/scrimmage/plugins/interaction/RandomAttrit/RandomAttrit.h
index ae31788527..12480a060e 100644
--- a/include/scrimmage/plugins/interaction/RandomAttrit/RandomAttrit.h
+++ b/include/scrimmage/plugins/interaction/RandomAttrit/RandomAttrit.h
@@ -32,14 +32,14 @@
 #ifndef INCLUDE_SCRIMMAGE_PLUGINS_INTERACTION_RANDOMATTRIT_RANDOMATTRIT_H_
 #define INCLUDE_SCRIMMAGE_PLUGINS_INTERACTION_RANDOMATTRIT_RANDOMATTRIT_H_
 
-#include <scrimmage/simcontrol/EntityInteraction.h>
-#include <scrimmage/entity/Entity.h>
 #include <scrimmage/common/Random.h>
+#include <scrimmage/entity/Entity.h>
 #include <scrimmage/pubsub/PubSub.h>
 #include <scrimmage/pubsub/Publisher.h>
+#include <scrimmage/simcontrol/EntityInteraction.h>
 
-#include <map>
 #include <list>
+#include <map>
 #include <string>
 #include <vector>
 
@@ -49,10 +49,11 @@ namespace interaction {
 class RandomAttrit : public scrimmage::EntityInteraction {
  public:
     RandomAttrit();
-    bool init(std::map<std::string, std::string> &mission_params,
-              std::map<std::string, std::string> &plugin_params) override;
-    bool step_entity_interaction(std::list<scrimmage::EntityPtr> &ents,
-                                 double t, double dt) override;
+    bool init(std::map<std::string, std::string>& mission_params,
+              std::map<std::string, std::string>& plugin_params) override;
+    bool step_entity_interaction(std::list<scrimmage::EntityPtr>& ents, double t,
+                                 double dt) override;
+
  protected:
     bool started_ = false;
     int team_id_ = -1;
@@ -66,8 +67,9 @@ class RandomAttrit : public scrimmage::EntityInteraction {
     Random random_;
 
     PublisherPtr attrit_pub_;
+
  private:
 };
-} // namespace interaction
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_INTERACTION_RANDOMATTRIT_RANDOMATTRIT_H_
+}  // namespace interaction
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_INTERACTION_RANDOMATTRIT_RANDOMATTRIT_H_
diff --git a/include/scrimmage/plugins/interaction/SimpleCapture/SimpleCapture.h b/include/scrimmage/plugins/interaction/SimpleCapture/SimpleCapture.h
index 1111892cc9..d46cbcd42d 100644
--- a/include/scrimmage/plugins/interaction/SimpleCapture/SimpleCapture.h
+++ b/include/scrimmage/plugins/interaction/SimpleCapture/SimpleCapture.h
@@ -35,20 +35,21 @@
 
 #include <scrimmage/simcontrol/EntityInteraction.h>
 
+#include <list>
 #include <map>
-#include <unordered_set>
 #include <string>
-#include <list>
+#include <unordered_set>
 
 namespace scrimmage {
 namespace interaction {
 class SimpleCapture : public scrimmage::EntityInteraction {
  public:
     SimpleCapture();
-    bool init(std::map<std::string, std::string> &mission_params,
-              std::map<std::string, std::string> &plugin_params) override;
-    bool step_entity_interaction(std::list<scrimmage::EntityPtr> &ents,
-                                 double t, double dt) override;
+    bool init(std::map<std::string, std::string>& mission_params,
+              std::map<std::string, std::string>& plugin_params) override;
+    bool step_entity_interaction(std::list<scrimmage::EntityPtr>& ents, double t,
+                                 double dt) override;
+
  protected:
     double capture_range_;
     bool enable_team_captures_;
diff --git a/include/scrimmage/plugins/interaction/SimpleCollision/SimpleCollision.h b/include/scrimmage/plugins/interaction/SimpleCollision/SimpleCollision.h
index 98ff8075da..594cce4b22 100644
--- a/include/scrimmage/plugins/interaction/SimpleCollision/SimpleCollision.h
+++ b/include/scrimmage/plugins/interaction/SimpleCollision/SimpleCollision.h
@@ -33,9 +33,9 @@
 #ifndef INCLUDE_SCRIMMAGE_PLUGINS_INTERACTION_SIMPLECOLLISION_SIMPLECOLLISION_H_
 #define INCLUDE_SCRIMMAGE_PLUGINS_INTERACTION_SIMPLECOLLISION_SIMPLECOLLISION_H_
 
-#include <scrimmage/simcontrol/EntityInteraction.h>
 #include <scrimmage/entity/Entity.h>
 #include <scrimmage/pubsub/Publisher.h>
+#include <scrimmage/simcontrol/EntityInteraction.h>
 
 #include <list>
 #include <map>
@@ -45,14 +45,13 @@ namespace scrimmage {
 namespace interaction {
 class SimpleCollision : public scrimmage::EntityInteraction {
  public:
-    bool init(std::map<std::string, std::string> &mission_params,
-                      std::map<std::string, std::string> &plugin_params) override;
+    bool init(std::map<std::string, std::string>& mission_params,
+              std::map<std::string, std::string>& plugin_params) override;
 
-    bool step_entity_interaction(
-        std::list<scrimmage::EntityPtr> &ents, double t, double dt) override;
+    bool step_entity_interaction(std::list<scrimmage::EntityPtr>& ents, double t,
+                                 double dt) override;
 
-    bool collision_exists(
-        std::list<scrimmage::EntityPtr> &ents, Eigen::Vector3d &p) override;
+    bool collision_exists(std::list<scrimmage::EntityPtr>& ents, Eigen::Vector3d& p) override;
 
  protected:
     double collision_range_;
@@ -65,6 +64,6 @@ class SimpleCollision : public scrimmage::EntityInteraction {
     scrimmage::PublisherPtr team_collision_pub_;
     scrimmage::PublisherPtr non_team_collision_pub_;
 };
-} // namespace interaction
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_INTERACTION_SIMPLECOLLISION_SIMPLECOLLISION_H_
+}  // namespace interaction
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_INTERACTION_SIMPLECOLLISION_SIMPLECOLLISION_H_
diff --git a/include/scrimmage/plugins/interaction/TerrainGenerator/TerrainGenerator.h b/include/scrimmage/plugins/interaction/TerrainGenerator/TerrainGenerator.h
index b7e06112aa..cd581dfb0d 100644
--- a/include/scrimmage/plugins/interaction/TerrainGenerator/TerrainGenerator.h
+++ b/include/scrimmage/plugins/interaction/TerrainGenerator/TerrainGenerator.h
@@ -33,12 +33,12 @@
 #ifndef INCLUDE_SCRIMMAGE_PLUGINS_INTERACTION_TERRAINGENERATOR_TERRAINGENERATOR_H_
 #define INCLUDE_SCRIMMAGE_PLUGINS_INTERACTION_TERRAINGENERATOR_TERRAINGENERATOR_H_
 
-#include <scrimmage/simcontrol/EntityInteraction.h>
 #include <scrimmage/entity/Entity.h>
 #include <scrimmage/plugins/interaction/TerrainGenerator/TerrainMap.h>
+#include <scrimmage/simcontrol/EntityInteraction.h>
 
-#include <map>
 #include <list>
+#include <map>
 #include <string>
 
 namespace scrimmage {
@@ -47,10 +47,10 @@ namespace interaction {
 class TerrainGenerator : public scrimmage::EntityInteraction {
  public:
     TerrainGenerator();
-    bool init(std::map<std::string, std::string> &mission_params,
-              std::map<std::string, std::string> &plugin_params) override;
-    bool step_entity_interaction(std::list<scrimmage::EntityPtr> &ents,
-                                 double t, double dt) override;
+    bool init(std::map<std::string, std::string>& mission_params,
+              std::map<std::string, std::string>& plugin_params) override;
+    bool step_entity_interaction(std::list<scrimmage::EntityPtr>& ents, double t,
+                                 double dt) override;
 
  protected:
     TerrainMap map_;
@@ -60,6 +60,6 @@ class TerrainGenerator : public scrimmage::EntityInteraction {
 
  private:
 };
-} // namespace interaction
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_INTERACTION_TERRAINGENERATOR_TERRAINGENERATOR_H_
+}  // namespace interaction
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_INTERACTION_TERRAINGENERATOR_TERRAINGENERATOR_H_
diff --git a/include/scrimmage/plugins/interaction/TerrainGenerator/TerrainMap.h b/include/scrimmage/plugins/interaction/TerrainGenerator/TerrainMap.h
index a13ae59ffe..187707a164 100644
--- a/include/scrimmage/plugins/interaction/TerrainGenerator/TerrainMap.h
+++ b/include/scrimmage/plugins/interaction/TerrainGenerator/TerrainMap.h
@@ -33,18 +33,17 @@
 #ifndef INCLUDE_SCRIMMAGE_PLUGINS_INTERACTION_TERRAINGENERATOR_TERRAINMAP_H_
 #define INCLUDE_SCRIMMAGE_PLUGINS_INTERACTION_TERRAINGENERATOR_TERRAINMAP_H_
 
-#include <scrimmage/simcontrol/EntityInteraction.h>
 #include <scrimmage/entity/Entity.h>
-#include <scrimmage/proto/Shape.pb.h>
 #include <scrimmage/msgs/Terrain.pb.h>
+#include <scrimmage/proto/Shape.pb.h>
+#include <scrimmage/simcontrol/EntityInteraction.h>
 
-#include <vector>
-#include <string>
+#include <boost/optional.hpp>
 #include <limits>
-#include <random>
 #include <memory>
-
-#include <boost/optional.hpp>
+#include <random>
+#include <string>
+#include <vector>
 
 namespace scrimmage {
 namespace interaction {
@@ -55,17 +54,14 @@ class TerrainMap {
 
     TerrainMap();
     TerrainMap(std::shared_ptr<std::normal_distribution<double>> rng,
-               std::shared_ptr<std::default_random_engine> gener,
-               const Technique &technique,
-               const Eigen::Vector3d &center,
-               const double &x_length, const double &y_length,
-               const double &x_resolution, const double &y_resolution,
-               const double &z_min, const double &z_max,
-               const Eigen::Vector3d &color);
-    explicit TerrainMap(const scrimmage_msgs::Terrain &terrain);
+               std::shared_ptr<std::default_random_engine> gener, const Technique& technique,
+               const Eigen::Vector3d& center, const double& x_length, const double& y_length,
+               const double& x_resolution, const double& y_resolution, const double& z_min,
+               const double& z_max, const Eigen::Vector3d& color);
+    explicit TerrainMap(const scrimmage_msgs::Terrain& terrain);
     scrimmage::ShapePtr shape();
     scrimmage_msgs::Terrain proto();
-    boost::optional<double> height_at(const double &x, const double &y);
+    boost::optional<double> height_at(const double& x, const double& y);
 
  protected:
     bool generate();
@@ -98,10 +94,10 @@ class TerrainMap {
     };
 
     std::vector<std::vector<Node>> grid_;
-    double get_neighbor_avg(const int &row, const int &col);
+    double get_neighbor_avg(const int& row, const int& col);
 
  private:
 };
-} // namespace interaction
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_INTERACTION_TERRAINGENERATOR_TERRAINMAP_H_
+}  // namespace interaction
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_INTERACTION_TERRAINGENERATOR_TERRAINMAP_H_
diff --git a/include/scrimmage/plugins/metrics/CPA/CPA.h b/include/scrimmage/plugins/metrics/CPA/CPA.h
index 8338241871..66b49925d9 100644
--- a/include/scrimmage/plugins/metrics/CPA/CPA.h
+++ b/include/scrimmage/plugins/metrics/CPA/CPA.h
@@ -33,13 +33,13 @@
 #ifndef INCLUDE_SCRIMMAGE_PLUGINS_METRICS_CPA_CPA_H_
 #define INCLUDE_SCRIMMAGE_PLUGINS_METRICS_CPA_CPA_H_
 
-#include <scrimmage/metrics/Metrics.h>
 #include <scrimmage/common/CSV.h>
+#include <scrimmage/metrics/Metrics.h>
 
+#include <limits>
 #include <map>
 #include <string>
 #include <vector>
-#include <limits>
 
 namespace sc = scrimmage;
 
@@ -48,13 +48,26 @@ namespace metrics {
 
 class CPAData {
  public:
-    CPAData() {}
-    double distance() {return distance_;}
-    int closest_entity() {return closest_entity_;}
-    double time() {return time_;}
-    void set_distance(double dist) {distance_ = dist;}
-    void set_closest_entity(int id) {closest_entity_ = id;}
-    void set_time(double t) {time_ = t;}
+    CPAData() {
+    }
+    double distance() {
+        return distance_;
+    }
+    int closest_entity() {
+        return closest_entity_;
+    }
+    double time() {
+        return time_;
+    }
+    void set_distance(double dist) {
+        distance_ = dist;
+    }
+    void set_closest_entity(int id) {
+        closest_entity_ = id;
+    }
+    void set_time(double t) {
+        time_ = t;
+    }
 
  protected:
     double distance_ = std::numeric_limits<double>::infinity();
@@ -65,10 +78,11 @@ class CPAData {
 class CPA : public scrimmage::Metrics {
  public:
     CPA();
-    void init(std::map<std::string, std::string> &params) override;
+    void init(std::map<std::string, std::string>& params) override;
     bool step_metrics(double t, double dt) override;
     void calc_team_scores() override;
     void print_team_summaries() override;
+
  protected:
     std::map<std::string, std::string> params_;
 
@@ -76,13 +90,13 @@ class CPA : public scrimmage::Metrics {
     std::map<int, CPAData> cpa_map_;
     CSV csv_;
     bool initialized_{false};
-    bool team_cpa_{false};  // Whether checking within a team
-    int team_id_{0};  // If checking a specific team ID
+    bool team_cpa_{false};     // Whether checking within a team
+    int team_id_{0};           // If checking a specific team ID
     double min_time_s_{-1.0};  // For constraining after some event
     double max_time_s_{-1.0};  // For constraining before some event
  private:
 };
 
-} // namespace metrics
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_METRICS_CPA_CPA_H_
+}  // namespace metrics
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_METRICS_CPA_CPA_H_
diff --git a/include/scrimmage/plugins/metrics/FlagCaptureMetrics/FlagCaptureMetrics.h b/include/scrimmage/plugins/metrics/FlagCaptureMetrics/FlagCaptureMetrics.h
index 2ae3eb51f2..290464106e 100644
--- a/include/scrimmage/plugins/metrics/FlagCaptureMetrics/FlagCaptureMetrics.h
+++ b/include/scrimmage/plugins/metrics/FlagCaptureMetrics/FlagCaptureMetrics.h
@@ -46,29 +46,40 @@ namespace metrics {
 
 class Score {
  public:
-    bool set_weights(std::map<std::string, std::string> &params) {
+    bool set_weights(std::map<std::string, std::string>& params) {
         flags_taken_w_ = sc::get<double>("flags_taken_w", params, 0.0);
         flags_captured_w_ = sc::get<double>("flags_captured_w", params, 0.0);
         return true;
     }
 
-    void increment_flags_taken() { flags_taken_++; }
-    void add_flags_taken(int c) { flags_taken_ += c; }
-    int flags_taken() { return flags_taken_; }
+    void increment_flags_taken() {
+        flags_taken_++;
+    }
+    void add_flags_taken(int c) {
+        flags_taken_ += c;
+    }
+    int flags_taken() {
+        return flags_taken_;
+    }
     void set_flags_taken(int flags_taken) {
         flags_taken_ = flags_taken;
     }
 
-    void increment_flags_captured() { flags_captured_++; }
-    void add_flags_captured(int c) { flags_captured_ += c; }
-    int flags_captured() { return flags_captured_; }
+    void increment_flags_captured() {
+        flags_captured_++;
+    }
+    void add_flags_captured(int c) {
+        flags_captured_ += c;
+    }
+    int flags_captured() {
+        return flags_captured_;
+    }
     void set_flags_captured(int flags_captured) {
         flags_captured_ = flags_captured;
     }
 
     double score() {
-        double s = flags_taken() * flags_taken_w_ +
-            flags_captured() * flags_captured_w_;
+        double s = flags_taken() * flags_taken_w_ + flags_captured() * flags_captured_w_;
         return s;
     }
 
@@ -80,21 +91,22 @@ class Score {
     double flags_captured_w_ = 0.0;
 };
 
-
 class FlagCaptureMetrics : public scrimmage::Metrics {
  public:
     FlagCaptureMetrics();
-    void init(std::map<std::string, std::string> &params) override;
+    void init(std::map<std::string, std::string>& params) override;
     bool step_metrics(double t, double dt) override;
     void calc_team_scores() override;
     void print_team_summaries() override;
+
  protected:
     std::map<int, Score> scores_;
     std::map<int, Score> team_flag_scores_;
     std::map<std::string, std::string> params_;
+
  private:
 };
 
-} // namespace metrics
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_METRICS_FLAGCAPTUREMETRICS_FLAGCAPTUREMETRICS_H_
+}  // namespace metrics
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_METRICS_FLAGCAPTUREMETRICS_FLAGCAPTUREMETRICS_H_
diff --git a/include/scrimmage/plugins/metrics/OpenAIRewards/OpenAIRewards.h b/include/scrimmage/plugins/metrics/OpenAIRewards/OpenAIRewards.h
index 19f1b9791a..4671a1cd10 100644
--- a/include/scrimmage/plugins/metrics/OpenAIRewards/OpenAIRewards.h
+++ b/include/scrimmage/plugins/metrics/OpenAIRewards/OpenAIRewards.h
@@ -44,8 +44,10 @@ namespace metrics {
 class OpenAIRewards : public scrimmage::Metrics {
  public:
     OpenAIRewards();
-    void init(std::map<std::string, std::string> &params) override;
-    bool step_metrics(double /*t*/, double /*dt*/) override {return true;}
+    void init(std::map<std::string, std::string>& params) override;
+    bool step_metrics(double /*t*/, double /*dt*/) override {
+        return true;
+    }
     void print_team_summaries() override;
     void calc_team_scores() override;
 
@@ -54,6 +56,6 @@ class OpenAIRewards : public scrimmage::Metrics {
     std::map<size_t, double> rewards_;
 };
 
-} // namespace metrics
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_METRICS_OPENAIREWARDS_OPENAIREWARDS_H_
+}  // namespace metrics
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_METRICS_OPENAIREWARDS_OPENAIREWARDS_H_
diff --git a/include/scrimmage/plugins/metrics/SimpleCaptureMetrics/SimpleCaptureMetrics.h b/include/scrimmage/plugins/metrics/SimpleCaptureMetrics/SimpleCaptureMetrics.h
index 39e19dca6a..7f062ae798 100644
--- a/include/scrimmage/plugins/metrics/SimpleCaptureMetrics/SimpleCaptureMetrics.h
+++ b/include/scrimmage/plugins/metrics/SimpleCaptureMetrics/SimpleCaptureMetrics.h
@@ -36,8 +36,8 @@
 #include <scrimmage/metrics/Metrics.h>
 
 #include <iostream>
-#include <set>
 #include <map>
+#include <set>
 #include <string>
 
 using std::cout;
@@ -49,9 +49,10 @@ namespace scrimmage {
 namespace metrics {
 class Score {
  public:
-    Score() {}
+    Score() {
+    }
 
-    bool set_weights(std::map<std::string, std::string> &params) {
+    bool set_weights(std::map<std::string, std::string>& params) {
         double w = sc::get<double>("TeamCapture_weight", params, 0.0);
         weights_["TeamCapture"] = w;
 
@@ -93,7 +94,7 @@ class Score {
     double score() {
         // Apply weights to all scores
         double s = 0;
-        for (auto &kv : counts_) {
+        for (auto& kv : counts_) {
             auto it = weights_.find(kv.first);
             if (it != weights_.end()) {
                 s += kv.second * it->second;
@@ -112,7 +113,7 @@ class Score {
 
 class SimpleCaptureMetrics : public scrimmage::Metrics {
  public:
-    void init(std::map<std::string, std::string> &params) override;
+    void init(std::map<std::string, std::string>& params) override;
     bool step_metrics(double t, double dt) override;
     void calc_team_scores() override;
     void print_team_summaries() override;
@@ -125,6 +126,6 @@ class SimpleCaptureMetrics : public scrimmage::Metrics {
 
     std::map<std::string, std::string> params_;
 };
-} // namespace metrics
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_METRICS_SIMPLECAPTUREMETRICS_SIMPLECAPTUREMETRICS_H_
+}  // namespace metrics
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_METRICS_SIMPLECAPTUREMETRICS_SIMPLECAPTUREMETRICS_H_
diff --git a/include/scrimmage/plugins/metrics/SimpleCollisionMetrics/SimpleCollisionMetrics.h b/include/scrimmage/plugins/metrics/SimpleCollisionMetrics/SimpleCollisionMetrics.h
index 6831fb853e..d4262bb1c4 100644
--- a/include/scrimmage/plugins/metrics/SimpleCollisionMetrics/SimpleCollisionMetrics.h
+++ b/include/scrimmage/plugins/metrics/SimpleCollisionMetrics/SimpleCollisionMetrics.h
@@ -34,19 +34,19 @@
 #define INCLUDE_SCRIMMAGE_PLUGINS_METRICS_SIMPLECOLLISIONMETRICS_SIMPLECOLLISIONMETRICS_H_
 #include <scrimmage/metrics/Metrics.h>
 
-#include "SimpleCollisionScore.h"
-
 #include <map>
 #include <set>
 #include <string>
 
+#include "SimpleCollisionScore.h"
+
 namespace scrimmage {
 namespace metrics {
 class SimpleCollisionMetrics : public scrimmage::Metrics {
  public:
     SimpleCollisionMetrics();
 
-    void init(std::map<std::string, std::string> &params) override;
+    void init(std::map<std::string, std::string>& params) override;
     bool step_metrics(double t, double dt) override;
     void calc_team_scores() override;
     void print_team_summaries() override;
diff --git a/include/scrimmage/plugins/metrics/SimpleCollisionMetrics/SimpleCollisionScore.h b/include/scrimmage/plugins/metrics/SimpleCollisionMetrics/SimpleCollisionScore.h
index 561051b0f6..f043aa6b8a 100644
--- a/include/scrimmage/plugins/metrics/SimpleCollisionMetrics/SimpleCollisionScore.h
+++ b/include/scrimmage/plugins/metrics/SimpleCollisionMetrics/SimpleCollisionScore.h
@@ -40,36 +40,82 @@ namespace scrimmage {
 namespace metrics {
 class SimpleCollisionScore {
  public:
-    bool set_weights(std::map<std::string, std::string> &params);
+    bool set_weights(std::map<std::string, std::string>& params);
 
-    void set_flight_time_start(double t) { flight_time_start_ = t; }
-    void set_flight_time_end(double t) { flight_time_end_ = t; }
-    void increment_non_team_collisions() { non_team_collisions_++; }
-    void increment_team_collisions() { team_collisions_++; }
-    void increment_ground_collisions() { ground_collisions_++; }
+    void set_flight_time_start(double t) {
+        flight_time_start_ = t;
+    }
+    void set_flight_time_end(double t) {
+        flight_time_end_ = t;
+    }
+    void increment_non_team_collisions() {
+        non_team_collisions_++;
+    }
+    void increment_team_collisions() {
+        team_collisions_++;
+    }
+    void increment_ground_collisions() {
+        ground_collisions_++;
+    }
 
-    void add_non_team_collisions(int c) { non_team_collisions_ += c; }
-    void add_team_collisions(int c) { team_collisions_ += c; }
-    void add_ground_collisions(int c) { ground_collisions_ += c; }
-    void add_flight_time(double t) { flight_time_end_ += t; }
+    void add_non_team_collisions(int c) {
+        non_team_collisions_ += c;
+    }
+    void add_team_collisions(int c) {
+        team_collisions_ += c;
+    }
+    void add_ground_collisions(int c) {
+        ground_collisions_ += c;
+    }
+    void add_flight_time(double t) {
+        flight_time_end_ += t;
+    }
 
-    int non_team_collisions() { return non_team_collisions_; }
-    int team_collisions() { return team_collisions_; }
-    int ground_collisions() { return ground_collisions_; }
+    int non_team_collisions() {
+        return non_team_collisions_;
+    }
+    int team_collisions() {
+        return team_collisions_;
+    }
+    int ground_collisions() {
+        return ground_collisions_;
+    }
 
-    void set_max_flight_time(double t) { max_flight_time_ = t; }
-    void increment_entity_count() { entity_count_++; }
+    void set_max_flight_time(double t) {
+        max_flight_time_ = t;
+    }
+    void increment_entity_count() {
+        entity_count_++;
+    }
 
-    int entity_count() { return entity_count_; }
-    void set_entity_count(int entity_count) {entity_count_ = entity_count;}
-    void set_non_team_collisions(int non_team_collisions) {non_team_collisions_ = non_team_collisions;}
-    void set_team_collisions(int team_collisions) {team_collisions_ = team_collisions;}
-    void set_ground_collisions(int ground_collisions) {ground_collisions_ = ground_collisions;}
+    int entity_count() {
+        return entity_count_;
+    }
+    void set_entity_count(int entity_count) {
+        entity_count_ = entity_count;
+    }
+    void set_non_team_collisions(int non_team_collisions) {
+        non_team_collisions_ = non_team_collisions;
+    }
+    void set_team_collisions(int team_collisions) {
+        team_collisions_ = team_collisions;
+    }
+    void set_ground_collisions(int ground_collisions) {
+        ground_collisions_ = ground_collisions;
+    }
 
-    double flight_time() {return (flight_time_end_ - flight_time_start_);}
-    double flight_time_end() {return flight_time_end_;}
-    double flight_time_start() {return flight_time_start_;}
-    double flight_time_norm() {return (flight_time() / max_flight_time_);}
+    double flight_time() {
+        return (flight_time_end_ - flight_time_start_);
+    }
+    double flight_time_end() {
+        return flight_time_end_;
+    }
+    double flight_time_start() {
+        return flight_time_start_;
+    }
+    double flight_time_norm() {
+        return (flight_time() / max_flight_time_);
+    }
 
     double score();
 
@@ -88,6 +134,6 @@ class SimpleCollisionScore {
     double max_flight_time_ = 0;
     int entity_count_ = 0;
 };
-} // namespace metrics
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_METRICS_SIMPLECOLLISIONMETRICS_SIMPLECOLLISIONSCORE_H_
+}  // namespace metrics
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_METRICS_SIMPLECOLLISIONMETRICS_SIMPLECOLLISIONSCORE_H_
diff --git a/include/scrimmage/plugins/motion/Ballistic/Ballistic.h b/include/scrimmage/plugins/motion/Ballistic/Ballistic.h
index 22692ed270..217da84cad 100644
--- a/include/scrimmage/plugins/motion/Ballistic/Ballistic.h
+++ b/include/scrimmage/plugins/motion/Ballistic/Ballistic.h
@@ -41,14 +41,14 @@ namespace scrimmage {
 namespace motion {
 class Ballistic : public scrimmage::MotionModel {
  public:
-    bool init(std::map<std::string, std::string> &info,
-              std::map<std::string, std::string> &params) override;
+    bool init(std::map<std::string, std::string>& info,
+              std::map<std::string, std::string>& params) override;
     bool step(double time, double dt) override;
-    void model(const vector_t &x , vector_t &dxdt , double t) override;
+    void model(const vector_t& x, vector_t& dxdt, double t) override;
 
  protected:
  private:
 };
-} // namespace motion
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_MOTION_BALLISTIC_BALLISTIC_H_
+}  // namespace motion
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_MOTION_BALLISTIC_BALLISTIC_H_
diff --git a/include/scrimmage/plugins/motion/DoubleIntegrator/DoubleIntegrator.h b/include/scrimmage/plugins/motion/DoubleIntegrator/DoubleIntegrator.h
index 420f0184d2..206daad37e 100644
--- a/include/scrimmage/plugins/motion/DoubleIntegrator/DoubleIntegrator.h
+++ b/include/scrimmage/plugins/motion/DoubleIntegrator/DoubleIntegrator.h
@@ -48,14 +48,14 @@ class DoubleIntegrator : public scrimmage::MotionModel {
  public:
     DoubleIntegrator();
 
-    bool init(std::map<std::string, std::string> &info,
-              std::map<std::string, std::string> &params) override;
+    bool init(std::map<std::string, std::string>& info,
+              std::map<std::string, std::string>& params) override;
 
     bool step(double t, double dt) override;
 
-    void model(const vector_t &x, vector_t &dxdt, double t) override;
+    void model(const vector_t& x, vector_t& dxdt, double t) override;
 
-    void teleport(scrimmage::StatePtr &state) override;
+    void teleport(scrimmage::StatePtr& state) override;
 
  protected:
     double update_dvdt(double vel, double max_vel, double acc);
diff --git a/include/scrimmage/plugins/motion/DubinsAirplane/DubinsAirplane.h b/include/scrimmage/plugins/motion/DubinsAirplane/DubinsAirplane.h
index 51838bbeaf..cf47624d7a 100644
--- a/include/scrimmage/plugins/motion/DubinsAirplane/DubinsAirplane.h
+++ b/include/scrimmage/plugins/motion/DubinsAirplane/DubinsAirplane.h
@@ -33,32 +33,32 @@
 #ifndef INCLUDE_SCRIMMAGE_PLUGINS_MOTION_DUBINSAIRPLANE_DUBINSAIRPLANE_H_
 #define INCLUDE_SCRIMMAGE_PLUGINS_MOTION_DUBINSAIRPLANE_DUBINSAIRPLANE_H_
 
+#include <scrimmage/common/CSV.h>
+#include <scrimmage/common/PID.h>
 #include <scrimmage/math/State.h>
-#include <scrimmage/motion/MotionModel.h>
 #include <scrimmage/motion/Controller.h>
-#include <scrimmage/common/PID.h>
-#include <scrimmage/common/CSV.h>
+#include <scrimmage/motion/MotionModel.h>
 
+#include <limits>
 #include <map>
 #include <string>
-#include <limits>
 
 namespace scrimmage {
 namespace motion {
 class DubinsAirplane : public scrimmage::MotionModel {
  public:
-    bool init(std::map<std::string, std::string> &info,
-              std::map<std::string, std::string> &params) override;
+    bool init(std::map<std::string, std::string>& info,
+              std::map<std::string, std::string>& params) override;
     bool step(double t, double dt) override;
-    void model(const vector_t &x , vector_t &dxdt , double t) override;
+    void model(const vector_t& x, vector_t& dxdt, double t) override;
 
  protected:
     double speed_max_ = +std::numeric_limits<double>::infinity();
     double speed_min_ = -std::numeric_limits<double>::infinity();
     double pitch_max_ = +90;
     double pitch_min_ = -90;
-    double roll_max_  = +90;
-    double roll_min_  = -90;
+    double roll_max_ = +90;
+    double roll_min_ = -90;
 
     bool write_csv_ = false;
     CSV csv_;
@@ -71,6 +71,6 @@ class DubinsAirplane : public scrimmage::MotionModel {
     double pitch_ = 0;
     double roll_ = 0;
 };
-} // namespace motion
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_MOTION_DUBINSAIRPLANE_DUBINSAIRPLANE_H_
+}  // namespace motion
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_MOTION_DUBINSAIRPLANE_DUBINSAIRPLANE_H_
diff --git a/include/scrimmage/plugins/motion/DubinsAirplane3D/DubinsAirplane3D.h b/include/scrimmage/plugins/motion/DubinsAirplane3D/DubinsAirplane3D.h
index 9f2964273f..1a5df4c2fb 100644
--- a/include/scrimmage/plugins/motion/DubinsAirplane3D/DubinsAirplane3D.h
+++ b/include/scrimmage/plugins/motion/DubinsAirplane3D/DubinsAirplane3D.h
@@ -33,24 +33,24 @@
 #ifndef INCLUDE_SCRIMMAGE_PLUGINS_MOTION_DUBINSAIRPLANE3D_DUBINSAIRPLANE3D_H_
 #define INCLUDE_SCRIMMAGE_PLUGINS_MOTION_DUBINSAIRPLANE3D_DUBINSAIRPLANE3D_H_
 
+#include <scrimmage/common/CSV.h>
+#include <scrimmage/common/PID.h>
 #include <scrimmage/math/State.h>
-#include <scrimmage/motion/MotionModel.h>
 #include <scrimmage/motion/Controller.h>
-#include <scrimmage/common/PID.h>
-#include <scrimmage/common/CSV.h>
+#include <scrimmage/motion/MotionModel.h>
 
+#include <limits>
 #include <map>
 #include <string>
-#include <limits>
 
 namespace scrimmage {
 namespace motion {
 class DubinsAirplane3D : public scrimmage::MotionModel {
  public:
-    bool init(std::map<std::string, std::string> &info,
-              std::map<std::string, std::string> &params) override;
+    bool init(std::map<std::string, std::string>& info,
+              std::map<std::string, std::string>& params) override;
     bool step(double t, double dt) override;
-    void model(const vector_t &x , vector_t &dxdt , double t) override;
+    void model(const vector_t& x, vector_t& dxdt, double t) override;
 
  protected:
     double speed_max_ = +std::numeric_limits<double>::infinity();
@@ -71,6 +71,6 @@ class DubinsAirplane3D : public scrimmage::MotionModel {
     double pitch_ = 0;
     double roll_ = 0;
 };
-} // namespace motion
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_MOTION_DUBINSAIRPLANE3D_DUBINSAIRPLANE3D_H_
+}  // namespace motion
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_MOTION_DUBINSAIRPLANE3D_DUBINSAIRPLANE3D_H_
diff --git a/include/scrimmage/plugins/motion/FixedWing6DOF/FixedWing6DOF.h b/include/scrimmage/plugins/motion/FixedWing6DOF/FixedWing6DOF.h
index ed5da20c3f..84bc9a1b0f 100644
--- a/include/scrimmage/plugins/motion/FixedWing6DOF/FixedWing6DOF.h
+++ b/include/scrimmage/plugins/motion/FixedWing6DOF/FixedWing6DOF.h
@@ -77,13 +77,13 @@ class FixedWing6DOF : public scrimmage::motion::RigidBody6DOFBase {
 
     virtual std::tuple<int, int, int> version();
 
-    bool init(std::map<std::string, std::string> &info,
-              std::map<std::string, std::string> &params) override;
+    bool init(std::map<std::string, std::string>& info,
+              std::map<std::string, std::string>& params) override;
     bool step(double time, double dt) override;
 
-    void teleport(StatePtr &state) override;
+    void teleport(StatePtr& state) override;
 
-    void model(const vector_t &x, vector_t &dxdt, double t) override;
+    void model(const vector_t& x, vector_t& dxdt, double t) override;
 
     enum LaunchState { PRELAUNCH = 0, LAUNCH, POSTLAUNCH };
 
diff --git a/include/scrimmage/plugins/motion/HarmonicOscillator/HarmonicOscillator.h b/include/scrimmage/plugins/motion/HarmonicOscillator/HarmonicOscillator.h
index 128c8106f6..30fe4c8fa2 100644
--- a/include/scrimmage/plugins/motion/HarmonicOscillator/HarmonicOscillator.h
+++ b/include/scrimmage/plugins/motion/HarmonicOscillator/HarmonicOscillator.h
@@ -40,10 +40,10 @@ namespace scrimmage {
 namespace motion {
 class HarmonicOscillator : public scrimmage::MotionModel {
  public:
-    bool init(std::map<std::string, std::string> &info,
-              std::map<std::string, std::string> &params) override;
+    bool init(std::map<std::string, std::string>& info,
+              std::map<std::string, std::string>& params) override;
     bool step(double time, double dt) override;
-    void model(const vector_t &x , vector_t &dxdt , double t) override;
+    void model(const vector_t& x, vector_t& dxdt, double t) override;
 
  protected:
     uint8_t acceleration_z_idx_ = 0;
@@ -53,6 +53,6 @@ class HarmonicOscillator : public scrimmage::MotionModel {
 
  private:
 };
-} // namespace motion
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_MOTION_HARMONICOSCILLATOR_HARMONICOSCILLATOR_H_
+}  // namespace motion
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_MOTION_HARMONICOSCILLATOR_HARMONICOSCILLATOR_H_
diff --git a/include/scrimmage/plugins/motion/JSBSimControl/JSBSimControl.h b/include/scrimmage/plugins/motion/JSBSimControl/JSBSimControl.h
index 946bbb4686..8e43f660de 100644
--- a/include/scrimmage/plugins/motion/JSBSimControl/JSBSimControl.h
+++ b/include/scrimmage/plugins/motion/JSBSimControl/JSBSimControl.h
@@ -33,93 +33,93 @@
 #ifndef INCLUDE_SCRIMMAGE_PLUGINS_MOTION_JSBSIMCONTROL_JSBSIMCONTROL_H_
 #define INCLUDE_SCRIMMAGE_PLUGINS_MOTION_JSBSIMCONTROL_JSBSIMCONTROL_H_
 
-#include <scrimmage/plugins/motion/RigidBody6DOF/RigidBody6DOFBase.h>
-
+#include <scrimmage/entity/Entity.h>
 #include <scrimmage/math/Angles.h>
-#include <scrimmage/motion/MotionModel.h>
 #include <scrimmage/motion/Controller.h>
-#include <scrimmage/entity/Entity.h>
+#include <scrimmage/motion/MotionModel.h>
+#include <scrimmage/plugins/motion/RigidBody6DOF/RigidBody6DOFBase.h>
+
 #include <Eigen/Dense>
 
 #if ENABLE_JSBSIM == 1
 #include <FGFDMExec.h>
-#include <models/FGAircraft.h>
-#include <input_output/FGPropertyManager.h>
 #include <initialization/FGInitialCondition.h>
+#include <input_output/FGPropertyManager.h>
+#include <models/FGAircraft.h>
 #include <models/FGOutput.h>
 
 typedef std::shared_ptr<JSBSim::FGFDMExec> FGFDMExecPtr;
 #endif
 
 #include <map>
+#include <memory>
 #include <string>
 #include <tuple>
-#include <memory>
 
 namespace scrimmage {
 namespace motion {
 class JSBSimControl : public scrimmage::motion::RigidBody6DOFBase {
  public:
-     JSBSimControl();
+    JSBSimControl();
 
-     virtual std::tuple<int, int, int> version();
+    virtual std::tuple<int, int, int> version();
 
-     bool init(std::map<std::string, std::string> &info,
-                       std::map<std::string, std::string> &params) override;
-     bool step(double time, double dt) override;
+    bool init(std::map<std::string, std::string>& info,
+              std::map<std::string, std::string>& params) override;
+    bool step(double time, double dt) override;
 
  protected:
 #if ENABLE_JSBSIM == 1
-     FGFDMExecPtr exec_;
-
-     JSBSim::FGPropertyNode *longitude_node_ = nullptr;
-     JSBSim::FGPropertyNode *latitude_node_ = nullptr;
-     JSBSim::FGPropertyNode *altitude_node_ = nullptr;
-     JSBSim::FGPropertyNode *altitudeAGL_node_ = nullptr;
-
-     JSBSim::FGPropertyNode *roll_node_ = nullptr;
-     JSBSim::FGPropertyNode *pitch_node_ = nullptr;
-     JSBSim::FGPropertyNode *yaw_node_ = nullptr;
-
-     JSBSim::FGPropertyNode *ap_aileron_cmd_node_ = nullptr;
-     JSBSim::FGPropertyNode *ap_elevator_cmd_node_ = nullptr;
-     JSBSim::FGPropertyNode *ap_rudder_cmd_node_ = nullptr;
-     JSBSim::FGPropertyNode *ap_throttle_cmd_node_ = nullptr;
-
-     JSBSim::FGPropertyNode *vel_north_node_ = nullptr;
-     JSBSim::FGPropertyNode *vel_east_node_ = nullptr;
-     JSBSim::FGPropertyNode *vel_down_node_ = nullptr;
-     JSBSim::FGPropertyNode *u_vel_node_ = nullptr;
-
-     JSBSim::FGPropertyNode *p_node_ = nullptr;
-     JSBSim::FGPropertyNode *q_node_ = nullptr;
-     JSBSim::FGPropertyNode *r_node_ = nullptr;
-
-     JSBSim::FGPropertyNode *ax_pilot_node_ = nullptr;
-     JSBSim::FGPropertyNode *ay_pilot_node_ = nullptr;
-     JSBSim::FGPropertyNode *az_pilot_node_ = nullptr;
-
-     scrimmage::Angles angles_to_jsbsim_;
-     scrimmage::Angles angles_from_jsbsim_;
-
-     JSBSim::FGOutputType* output_fg_ = 0;
-     bool fg_out_enable_ = false;
-
-     int throttle_idx_ = 0;
-     int elevator_idx_ = 0;
-     int aileron_idx_ = 0;
-     int rudder_idx_ = 0;
-
-     double throttle_ = 0;
-     double delta_elevator_ = 0;
-     double delta_aileron_ = 0;
-     double delta_rudder_ = 0;
-
-     double draw_vel_ = 0;
-     double draw_ang_vel_ = 0;
-     double draw_acc_ = 0;
+    FGFDMExecPtr exec_;
+
+    JSBSim::FGPropertyNode* longitude_node_ = nullptr;
+    JSBSim::FGPropertyNode* latitude_node_ = nullptr;
+    JSBSim::FGPropertyNode* altitude_node_ = nullptr;
+    JSBSim::FGPropertyNode* altitudeAGL_node_ = nullptr;
+
+    JSBSim::FGPropertyNode* roll_node_ = nullptr;
+    JSBSim::FGPropertyNode* pitch_node_ = nullptr;
+    JSBSim::FGPropertyNode* yaw_node_ = nullptr;
+
+    JSBSim::FGPropertyNode* ap_aileron_cmd_node_ = nullptr;
+    JSBSim::FGPropertyNode* ap_elevator_cmd_node_ = nullptr;
+    JSBSim::FGPropertyNode* ap_rudder_cmd_node_ = nullptr;
+    JSBSim::FGPropertyNode* ap_throttle_cmd_node_ = nullptr;
+
+    JSBSim::FGPropertyNode* vel_north_node_ = nullptr;
+    JSBSim::FGPropertyNode* vel_east_node_ = nullptr;
+    JSBSim::FGPropertyNode* vel_down_node_ = nullptr;
+    JSBSim::FGPropertyNode* u_vel_node_ = nullptr;
+
+    JSBSim::FGPropertyNode* p_node_ = nullptr;
+    JSBSim::FGPropertyNode* q_node_ = nullptr;
+    JSBSim::FGPropertyNode* r_node_ = nullptr;
+
+    JSBSim::FGPropertyNode* ax_pilot_node_ = nullptr;
+    JSBSim::FGPropertyNode* ay_pilot_node_ = nullptr;
+    JSBSim::FGPropertyNode* az_pilot_node_ = nullptr;
+
+    scrimmage::Angles angles_to_jsbsim_;
+    scrimmage::Angles angles_from_jsbsim_;
+
+    JSBSim::FGOutputType* output_fg_ = 0;
+    bool fg_out_enable_ = false;
+
+    int throttle_idx_ = 0;
+    int elevator_idx_ = 0;
+    int aileron_idx_ = 0;
+    int rudder_idx_ = 0;
+
+    double throttle_ = 0;
+    double delta_elevator_ = 0;
+    double delta_aileron_ = 0;
+    double delta_rudder_ = 0;
+
+    double draw_vel_ = 0;
+    double draw_ang_vel_ = 0;
+    double draw_acc_ = 0;
 #endif
 };
-} // namespace motion
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_MOTION_JSBSIMCONTROL_JSBSIMCONTROL_H_
+}  // namespace motion
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_MOTION_JSBSIMCONTROL_JSBSIMCONTROL_H_
diff --git a/include/scrimmage/plugins/motion/JSBSimModel/FGOutputFGMod.h b/include/scrimmage/plugins/motion/JSBSimModel/FGOutputFGMod.h
index 592f7f4231..ad3a950a02 100644
--- a/include/scrimmage/plugins/motion/JSBSimModel/FGOutputFGMod.h
+++ b/include/scrimmage/plugins/motion/JSBSimModel/FGOutputFGMod.h
@@ -40,10 +40,11 @@ class FGOutputFGMod : public FGOutputFG {
     /// Constructor
     explicit FGOutputFGMod(FGFDMExec* fdmex);
     virtual void Print(void);
+
  protected:
  private:
     FGNetFDM fgSockBufMod;
     void SocketDataFillMod(FGNetFDM* net);
 };
-} // namespace JSBSim
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_MOTION_JSBSIMMODEL_FGOUTPUTFGMOD_H_
+}  // namespace JSBSim
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_MOTION_JSBSIMMODEL_FGOUTPUTFGMOD_H_
diff --git a/include/scrimmage/plugins/motion/JSBSimModel/JSBSimModel.h b/include/scrimmage/plugins/motion/JSBSimModel/JSBSimModel.h
index 2850ac09d5..e25e8c21b6 100644
--- a/include/scrimmage/plugins/motion/JSBSimModel/JSBSimModel.h
+++ b/include/scrimmage/plugins/motion/JSBSimModel/JSBSimModel.h
@@ -33,29 +33,29 @@
 #ifndef INCLUDE_SCRIMMAGE_PLUGINS_MOTION_JSBSIMMODEL_JSBSIMMODEL_H_
 #define INCLUDE_SCRIMMAGE_PLUGINS_MOTION_JSBSIMMODEL_JSBSIMMODEL_H_
 
-#include <scrimmage/motion/MotionModel.h>
 #include <scrimmage/common/PID.h>
 #include <scrimmage/math/Angles.h>
+#include <scrimmage/motion/MotionModel.h>
 
 #if ENABLE_JSBSIM == 1
 #include <FGFDMExec.h>
-#include <models/FGAircraft.h>
-#include <input_output/FGPropertyManager.h>
 #include <initialization/FGInitialCondition.h>
+#include <input_output/FGPropertyManager.h>
+#include <models/FGAircraft.h>
 #include <models/FGOutput.h>
 
 typedef std::shared_ptr<JSBSim::FGFDMExec> FGFDMExecPtr;
 #endif
 
 #include <map>
+#include <memory>
 #include <string>
 #include <tuple>
-#include <memory>
 
 namespace scrimmage {
 class State;
 using StatePtr = std::shared_ptr<State>;
-}
+}  // namespace scrimmage
 
 namespace scrimmage {
 namespace motion {
@@ -64,44 +64,46 @@ class JSBSimModel : public MotionModel {
  public:
     virtual std::tuple<int, int, int> version();
 
-    virtual bool init(std::map<std::string, std::string> &info,
-                      std::map<std::string, std::string> &params);
+    virtual bool init(std::map<std::string, std::string>& info,
+                      std::map<std::string, std::string>& params);
     virtual bool step(double time, double dt);
 
-    virtual void teleport(StatePtr &state);
+    virtual void teleport(StatePtr& state);
 
-    bool use_pitch() { return use_pitch_; }
+    bool use_pitch() {
+        return use_pitch_;
+    }
 
  protected:
 #if ENABLE_JSBSIM == 1
     FGFDMExecPtr exec_;
 
-    JSBSim::FGPropertyNode *longitude_node_;
-    JSBSim::FGPropertyNode *latitude_node_;
-    JSBSim::FGPropertyNode *altitude_node_;
+    JSBSim::FGPropertyNode* longitude_node_;
+    JSBSim::FGPropertyNode* latitude_node_;
+    JSBSim::FGPropertyNode* altitude_node_;
 
-    JSBSim::FGPropertyNode *roll_node_;
-    JSBSim::FGPropertyNode *pitch_node_;
-    JSBSim::FGPropertyNode *yaw_node_;
+    JSBSim::FGPropertyNode* roll_node_;
+    JSBSim::FGPropertyNode* pitch_node_;
+    JSBSim::FGPropertyNode* yaw_node_;
 
-    JSBSim::FGPropertyNode *fcs_elevator_cmd_node_;
+    JSBSim::FGPropertyNode* fcs_elevator_cmd_node_;
 
-    JSBSim::FGPropertyNode *desired_heading_node_;
-    JSBSim::FGPropertyNode *desired_altitude_node_;
-    JSBSim::FGPropertyNode *desired_velocity_node_;
-    JSBSim::FGPropertyNode *bank_setpoint_node_;
+    JSBSim::FGPropertyNode* desired_heading_node_;
+    JSBSim::FGPropertyNode* desired_altitude_node_;
+    JSBSim::FGPropertyNode* desired_velocity_node_;
+    JSBSim::FGPropertyNode* bank_setpoint_node_;
 
-    JSBSim::FGPropertyNode *vel_north_node_;
-    JSBSim::FGPropertyNode *vel_east_node_;
-    JSBSim::FGPropertyNode *vel_down_node_;
+    JSBSim::FGPropertyNode* vel_north_node_;
+    JSBSim::FGPropertyNode* vel_east_node_;
+    JSBSim::FGPropertyNode* vel_down_node_;
 
-    JSBSim::FGPropertyNode *p_node_ = nullptr;
-    JSBSim::FGPropertyNode *q_node_ = nullptr;
-    JSBSim::FGPropertyNode *r_node_ = nullptr;
+    JSBSim::FGPropertyNode* p_node_ = nullptr;
+    JSBSim::FGPropertyNode* q_node_ = nullptr;
+    JSBSim::FGPropertyNode* r_node_ = nullptr;
 
-    JSBSim::FGPropertyNode *ax_pilot_node_ = nullptr;
-    JSBSim::FGPropertyNode *ay_pilot_node_ = nullptr;
-    JSBSim::FGPropertyNode *az_pilot_node_ = nullptr;
+    JSBSim::FGPropertyNode* ax_pilot_node_ = nullptr;
+    JSBSim::FGPropertyNode* ay_pilot_node_ = nullptr;
+    JSBSim::FGPropertyNode* az_pilot_node_ = nullptr;
 
     JSBSim::FGOutputType* output_fg_ = 0;
 
@@ -122,6 +124,6 @@ class JSBSimModel : public MotionModel {
 
     bool fg_out_enable_ = false;
 };
-} // namespace motion
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_MOTION_JSBSIMMODEL_JSBSIMMODEL_H_
+}  // namespace motion
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_MOTION_JSBSIMMODEL_JSBSIMMODEL_H_
diff --git a/include/scrimmage/plugins/motion/Multirotor/Multirotor.h b/include/scrimmage/plugins/motion/Multirotor/Multirotor.h
index 598e00f3ac..b6fb44f2d7 100644
--- a/include/scrimmage/plugins/motion/Multirotor/Multirotor.h
+++ b/include/scrimmage/plugins/motion/Multirotor/Multirotor.h
@@ -71,23 +71,31 @@ class Multirotor : public scrimmage::motion::RigidBody6DOFBase {
 
     Multirotor();
 
-    bool init(std::map<std::string, std::string> &info,
-              std::map<std::string, std::string> &params) override;
+    bool init(std::map<std::string, std::string>& info,
+              std::map<std::string, std::string>& params) override;
     bool step(double time, double dt) override;
 
-    void model(const vector_t &x, vector_t &dxdt, double t) override;
+    void model(const vector_t& x, vector_t& dxdt, double t) override;
 
     class Controller : public scrimmage::Controller {
      public:
-        virtual Eigen::VectorXd &u() = 0;
+        virtual Eigen::VectorXd& u() = 0;
     };
 
-    std::vector<Rotor> &rotors() { return rotors_; }
+    std::vector<Rotor>& rotors() {
+        return rotors_;
+    }
 
-    double omega_max() { return wmax_; }
-    double omega_min() { return wmin_; }
+    double omega_max() {
+        return wmax_;
+    }
+    double omega_min() {
+        return wmin_;
+    }
 
-    double c_T() { return c_T_; }
+    double c_T() {
+        return c_T_;
+    }
 
  protected:
     Eigen::VectorXd ctrl_u_;
diff --git a/include/scrimmage/plugins/motion/Multirotor/MultirotorState.h b/include/scrimmage/plugins/motion/Multirotor/MultirotorState.h
index ded8a79788..a197568065 100644
--- a/include/scrimmage/plugins/motion/Multirotor/MultirotorState.h
+++ b/include/scrimmage/plugins/motion/Multirotor/MultirotorState.h
@@ -36,7 +36,6 @@
 #include <scrimmage/math/State.h>
 
 #include <Eigen/Dense>
-
 #include <memory>
 
 namespace scrimmage {
@@ -46,13 +45,21 @@ class MultirotorState : public scrimmage::State {
  public:
     enum InputType { OMEGA = 0, PWM };
 
-    void set_input_type(InputType input_type) { input_type_ = input_type; }
+    void set_input_type(InputType input_type) {
+        input_type_ = input_type;
+    }
 
-    void set_prop_input(Eigen::VectorXd prop_input) { prop_input_ = prop_input; }
+    void set_prop_input(Eigen::VectorXd prop_input) {
+        prop_input_ = prop_input;
+    }
 
-    InputType &input_type() { return input_type_; }
+    InputType& input_type() {
+        return input_type_;
+    }
 
-    Eigen::VectorXd &prop_input() { return prop_input_; }
+    Eigen::VectorXd& prop_input() {
+        return prop_input_;
+    }
 
  protected:
     Eigen::VectorXd prop_input_;
diff --git a/include/scrimmage/plugins/motion/Multirotor/Rotor.h b/include/scrimmage/plugins/motion/Multirotor/Rotor.h
index 93aceb48aa..010dfa95b1 100644
--- a/include/scrimmage/plugins/motion/Multirotor/Rotor.h
+++ b/include/scrimmage/plugins/motion/Multirotor/Rotor.h
@@ -46,24 +46,36 @@ class Rotor {
  public:
     enum Direction { CW = 1, CCW = -1 };
 
-    Rotor()
-        : dir_(Direction::CW),
-          offset_length_(1.0),
-          xy_angle_(0.0) {}
+    Rotor() : dir_(Direction::CW), offset_length_(1.0), xy_angle_(0.0) {
+    }
 
-    void set_direction(Direction dir) { dir_ = dir; }
+    void set_direction(Direction dir) {
+        dir_ = dir;
+    }
     void set_offset(Eigen::Vector3d offset) {
         offset_ = offset;
         offset_length_ = offset_.norm();
         xy_angle_ = atan2(offset_(1), offset_(0));
     }
-    void set_quat(scrimmage::Quaternion quat) { quat_ = quat; }
+    void set_quat(scrimmage::Quaternion quat) {
+        quat_ = quat;
+    }
 
-    Direction &direction() { return dir_; }
-    Eigen::Vector3d &offset() { return offset_; }
-    scrimmage::Quaternion &quat() { return quat_; }
-    double offset_length() { return offset_length_; }
-    double xy_angle() { return xy_angle_; }
+    Direction& direction() {
+        return dir_;
+    }
+    Eigen::Vector3d& offset() {
+        return offset_;
+    }
+    scrimmage::Quaternion& quat() {
+        return quat_;
+    }
+    double offset_length() {
+        return offset_length_;
+    }
+    double xy_angle() {
+        return xy_angle_;
+    }
 
  protected:
     Direction dir_;
diff --git a/include/scrimmage/plugins/motion/RigidBody6DOF/RigidBody6DOF.h b/include/scrimmage/plugins/motion/RigidBody6DOF/RigidBody6DOF.h
index 936a082c63..f65a99ce74 100644
--- a/include/scrimmage/plugins/motion/RigidBody6DOF/RigidBody6DOF.h
+++ b/include/scrimmage/plugins/motion/RigidBody6DOF/RigidBody6DOF.h
@@ -32,29 +32,29 @@
 
 #ifndef INCLUDE_SCRIMMAGE_PLUGINS_MOTION_RIGIDBODY6DOF_RIGIDBODY6DOF_H_
 #define INCLUDE_SCRIMMAGE_PLUGINS_MOTION_RIGIDBODY6DOF_RIGIDBODY6DOF_H_
+#include <scrimmage/common/PID.h>
 #include <scrimmage/math/State.h>
-#include <scrimmage/motion/MotionModel.h>
 #include <scrimmage/motion/Controller.h>
-#include <scrimmage/common/PID.h>
+#include <scrimmage/motion/MotionModel.h>
 
 #include <map>
+#include <memory>
 #include <string>
 #include <tuple>
-#include <memory>
 
 namespace scrimmage {
 namespace motion {
-class RigidBody6DOF : public scrimmage::MotionModel{
+class RigidBody6DOF : public scrimmage::MotionModel {
  public:
     virtual std::tuple<int, int, int> version();
 
-    bool init(std::map<std::string, std::string> &info,
-                      std::map<std::string, std::string> &params) override;
+    bool init(std::map<std::string, std::string>& info,
+              std::map<std::string, std::string>& params) override;
     bool step(double time, double dt) override;
 
-    void model(const vector_t &x , vector_t &dxdt , double t) override;
+    void model(const vector_t& x, vector_t& dxdt, double t) override;
 
-    void teleport(scrimmage::StatePtr &state) override;
+    void teleport(scrimmage::StatePtr& state) override;
 
     class Controller : public scrimmage::Controller {
      public:
@@ -62,7 +62,9 @@ class RigidBody6DOF : public scrimmage::MotionModel{
     };
 
     // cppcheck-suppress passedByValue
-    void set_u(std::shared_ptr<Eigen::Vector4d> u) {ctrl_u_ = u;}
+    void set_u(std::shared_ptr<Eigen::Vector4d> u) {
+        ctrl_u_ = u;
+    }
 
  protected:
     scrimmage::PID heading_pid_;
@@ -80,6 +82,6 @@ class RigidBody6DOF : public scrimmage::MotionModel{
     scrimmage::Quaternion quat_world_;
     scrimmage::Quaternion quat_local_;
 };
-} // namespace motion
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_MOTION_RIGIDBODY6DOF_RIGIDBODY6DOF_H_
+}  // namespace motion
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_MOTION_RIGIDBODY6DOF_RIGIDBODY6DOF_H_
diff --git a/include/scrimmage/plugins/motion/RigidBody6DOF/RigidBody6DOFBase.h b/include/scrimmage/plugins/motion/RigidBody6DOF/RigidBody6DOFBase.h
index 80386ad3e1..ede432ce8c 100644
--- a/include/scrimmage/plugins/motion/RigidBody6DOF/RigidBody6DOFBase.h
+++ b/include/scrimmage/plugins/motion/RigidBody6DOF/RigidBody6DOFBase.h
@@ -38,29 +38,31 @@ namespace scrimmage {
 namespace motion {
 class RigidBody6DOFBase : public scrimmage::MotionModel {
  public:
-    RigidBody6DOFBase() : linear_vel_body_(0, 0, 0), ang_vel_body_(0, 0, 0),
-                          linear_accel_body_(0, 0, 0),
-                          ang_accel_body_(0, 0, 0),
-                          wind_(0, 0, 0) {
+    RigidBody6DOFBase()
+        : linear_vel_body_(0, 0, 0),
+          ang_vel_body_(0, 0, 0),
+          linear_accel_body_(0, 0, 0),
+          ang_accel_body_(0, 0, 0),
+          wind_(0, 0, 0) {
     }
 
-    Eigen::Vector3d &linear_vel_body() {
+    Eigen::Vector3d& linear_vel_body() {
         return linear_vel_body_;
     }
 
-    Eigen::Vector3d &ang_vel_body() {
+    Eigen::Vector3d& ang_vel_body() {
         return ang_vel_body_;
     }
 
-    Eigen::Vector3d &linear_accel_body() {
+    Eigen::Vector3d& linear_accel_body() {
         return linear_accel_body_;
     }
 
-    Eigen::Vector3d &ang_accel_body() {
+    Eigen::Vector3d& ang_accel_body() {
         return ang_accel_body_;
     }
 
-    Eigen::Vector3d &wind() {
+    Eigen::Vector3d& wind() {
         return wind_;
     }
 
@@ -75,6 +77,6 @@ class RigidBody6DOFBase : public scrimmage::MotionModel {
  public:
     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };
-} // namespace motion
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_MOTION_RIGIDBODY6DOF_RIGIDBODY6DOFBASE_H_
+}  // namespace motion
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_MOTION_RIGIDBODY6DOF_RIGIDBODY6DOFBASE_H_
diff --git a/include/scrimmage/plugins/motion/RigidBody6DOF/RigidBody6DOFState.h b/include/scrimmage/plugins/motion/RigidBody6DOF/RigidBody6DOFState.h
index 0f17c9e909..adf490a5e2 100644
--- a/include/scrimmage/plugins/motion/RigidBody6DOF/RigidBody6DOFState.h
+++ b/include/scrimmage/plugins/motion/RigidBody6DOF/RigidBody6DOFState.h
@@ -36,7 +36,6 @@
 #include <scrimmage/math/State.h>
 
 #include <Eigen/Dense>
-
 #include <memory>
 
 namespace scrimmage {
@@ -44,52 +43,54 @@ namespace motion {
 
 class RigidBody6DOFState : public scrimmage::State {
  public:
-    RigidBody6DOFState() : linear_vel_body_(0, 0, 0), ang_vel_body_(0, 0, 0),
-            linear_accel_body_(0, 0, 0), ang_accel_body_(0, 0, 0),
-            wind_(0, 0, 0) {
+    RigidBody6DOFState()
+        : linear_vel_body_(0, 0, 0),
+          ang_vel_body_(0, 0, 0),
+          linear_accel_body_(0, 0, 0),
+          ang_accel_body_(0, 0, 0),
+          wind_(0, 0, 0) {
     }
 
-    Eigen::Vector3d &linear_vel_body() {
+    Eigen::Vector3d& linear_vel_body() {
         return linear_vel_body_;
     }
 
-    Eigen::Vector3d &ang_vel_body() {
+    Eigen::Vector3d& ang_vel_body() {
         return ang_vel_body_;
     }
 
-    Eigen::Vector3d &linear_accel_body() {
+    Eigen::Vector3d& linear_accel_body() {
         return linear_accel_body_;
     }
 
-    Eigen::Vector3d &ang_accel_body() {
+    Eigen::Vector3d& ang_accel_body() {
         return ang_accel_body_;
     }
 
-    Eigen::Vector3d &wind() {
+    Eigen::Vector3d& wind() {
         return wind_;
     }
 
-    const Eigen::Vector3d &linear_vel_body_const() const {
+    const Eigen::Vector3d& linear_vel_body_const() const {
         return linear_vel_body_;
     }
 
-    const Eigen::Vector3d &ang_vel_body_const() const {
+    const Eigen::Vector3d& ang_vel_body_const() const {
         return ang_vel_body_;
     }
 
-    const Eigen::Vector3d &linear_accel_body_const() const {
+    const Eigen::Vector3d& linear_accel_body_const() const {
         return linear_accel_body_;
     }
 
-    const Eigen::Vector3d &ang_accel_body_const() const {
+    const Eigen::Vector3d& ang_accel_body_const() const {
         return ang_accel_body_;
     }
 
-    const Eigen::Vector3d &wind() const {
+    const Eigen::Vector3d& wind() const {
         return wind_;
     }
 
-
  protected:
     Eigen::Vector3d linear_vel_body_;
     Eigen::Vector3d ang_vel_body_;
@@ -104,6 +105,6 @@ class RigidBody6DOFState : public scrimmage::State {
 
 using RigidBody6DOFStatePtr = std::shared_ptr<RigidBody6DOFState>;
 
-} // namespace motion
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_MOTION_RIGIDBODY6DOF_RIGIDBODY6DOFSTATE_H_
+}  // namespace motion
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_MOTION_RIGIDBODY6DOF_RIGIDBODY6DOFSTATE_H_
diff --git a/include/scrimmage/plugins/motion/SimpleAircraft/SimpleAircraft.h b/include/scrimmage/plugins/motion/SimpleAircraft/SimpleAircraft.h
index 6156d7b45f..5f14542238 100644
--- a/include/scrimmage/plugins/motion/SimpleAircraft/SimpleAircraft.h
+++ b/include/scrimmage/plugins/motion/SimpleAircraft/SimpleAircraft.h
@@ -33,33 +33,33 @@
 #ifndef INCLUDE_SCRIMMAGE_PLUGINS_MOTION_SIMPLEAIRCRAFT_SIMPLEAIRCRAFT_H_
 #define INCLUDE_SCRIMMAGE_PLUGINS_MOTION_SIMPLEAIRCRAFT_SIMPLEAIRCRAFT_H_
 
-#include <scrimmage/motion/MotionModel.h>
-#include <scrimmage/motion/Controller.h>
 #include <scrimmage/common/PID.h>
+#include <scrimmage/motion/Controller.h>
+#include <scrimmage/motion/MotionModel.h>
 
 #include <map>
+#include <memory>
 #include <string>
 #include <tuple>
-#include <memory>
 
 namespace scrimmage {
 class State;
 using StatePtr = std::shared_ptr<State>;
-}
+}  // namespace scrimmage
 
 namespace scrimmage {
 namespace motion {
-class SimpleAircraft : public scrimmage::MotionModel{
+class SimpleAircraft : public scrimmage::MotionModel {
  public:
     virtual std::tuple<int, int, int> version();
 
-    bool init(std::map<std::string, std::string> &info,
-                      std::map<std::string, std::string> &params) override;
+    bool init(std::map<std::string, std::string>& info,
+              std::map<std::string, std::string>& params) override;
     bool step(double time, double dt) override;
 
-    void model(const vector_t &x , vector_t &dxdt , double t) override;
+    void model(const vector_t& x, vector_t& dxdt, double t) override;
 
-    void teleport(scrimmage::StatePtr &state) override;
+    void teleport(scrimmage::StatePtr& state) override;
 
  protected:
     scrimmage::PID heading_pid_;
@@ -67,8 +67,10 @@ class SimpleAircraft : public scrimmage::MotionModel{
     scrimmage::PID vel_pid_;
 
     double length_{50.0};  // Turning radius
-    double speedTarget_{50.0};  // The speed at which a full turn will result in the turning radius being met
-    // The slope at which the turning radius is adjusted based on speed.  Positive means larger turning radius at higher speeds.
+    double speedTarget_{
+        50.0};  // The speed at which a full turn will result in the turning radius being met
+    // The slope at which the turning radius is adjusted based on speed.  Positive means larger
+    // turning radius at higher speeds.
     double lengthSlopePerSpeed_{0.0};
 
     double min_velocity_{15.0};
@@ -82,6 +84,6 @@ class SimpleAircraft : public scrimmage::MotionModel{
     uint8_t roll_rate_idx_{0};
     uint8_t pitch_rate_idx_{0};
 };
-} // namespace motion
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_MOTION_SIMPLEAIRCRAFT_SIMPLEAIRCRAFT_H_
+}  // namespace motion
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_MOTION_SIMPLEAIRCRAFT_SIMPLEAIRCRAFT_H_
diff --git a/include/scrimmage/plugins/motion/SimpleCar/SimpleCar.h b/include/scrimmage/plugins/motion/SimpleCar/SimpleCar.h
index 721c7212f6..e8838e1130 100644
--- a/include/scrimmage/plugins/motion/SimpleCar/SimpleCar.h
+++ b/include/scrimmage/plugins/motion/SimpleCar/SimpleCar.h
@@ -42,11 +42,11 @@ namespace scrimmage {
 namespace motion {
 class SimpleCar : public scrimmage::MotionModel {
  public:
-    bool init(std::map<std::string, std::string> &info,
-                      std::map<std::string, std::string> &params) override;
+    bool init(std::map<std::string, std::string>& info,
+              std::map<std::string, std::string>& params) override;
     bool step(double time, double dt) override;
 
-    void model(const vector_t &x , vector_t &dxdt , double t) override;
+    void model(const vector_t& x, vector_t& dxdt, double t) override;
 
  protected:
     double length_;
@@ -56,6 +56,6 @@ class SimpleCar : public scrimmage::MotionModel {
     uint8_t input_speed_idx_;
     uint8_t input_turn_rate_idx_;
 };
-} // namespace motion
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_MOTION_SIMPLECAR_SIMPLECAR_H_
+}  // namespace motion
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_MOTION_SIMPLECAR_SIMPLECAR_H_
diff --git a/include/scrimmage/plugins/motion/SimpleQuadrotor/SimpleQuadrotor.h b/include/scrimmage/plugins/motion/SimpleQuadrotor/SimpleQuadrotor.h
index 2b1832e968..b519f7d8f5 100644
--- a/include/scrimmage/plugins/motion/SimpleQuadrotor/SimpleQuadrotor.h
+++ b/include/scrimmage/plugins/motion/SimpleQuadrotor/SimpleQuadrotor.h
@@ -32,33 +32,33 @@
 
 #ifndef INCLUDE_SCRIMMAGE_PLUGINS_MOTION_SIMPLEQUADROTOR_SIMPLEQUADROTOR_H_
 #define INCLUDE_SCRIMMAGE_PLUGINS_MOTION_SIMPLEQUADROTOR_SIMPLEQUADROTOR_H_
+#include <scrimmage/common/PID.h>
 #include <scrimmage/math/State.h>
-#include <scrimmage/motion/MotionModel.h>
 #include <scrimmage/motion/Controller.h>
-#include <scrimmage/common/PID.h>
+#include <scrimmage/motion/MotionModel.h>
 
 #include <map>
 #include <string>
 
 namespace scrimmage {
 namespace motion {
-class SimpleQuadrotor : public scrimmage::MotionModel{
+class SimpleQuadrotor : public scrimmage::MotionModel {
  public:
-    bool init(std::map<std::string, std::string> &info,
-                      std::map<std::string, std::string> &params) override;
+    bool init(std::map<std::string, std::string>& info,
+              std::map<std::string, std::string>& params) override;
     bool step(double time, double dt) override;
 
-    void model(const vector_t &x , vector_t &dxdt , double t) override;
+    void model(const vector_t& x, vector_t& dxdt, double t) override;
 
     class Controller : public scrimmage::Controller {
      public:
-        virtual Eigen::Vector4d &u() = 0;
+        virtual Eigen::Vector4d& u() = 0;
     };
 
  protected:
     double max_vel_;
     double max_pitch_;
 };
-} // namespace motion
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_MOTION_SIMPLEQUADROTOR_SIMPLEQUADROTOR_H_
+}  // namespace motion
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_MOTION_SIMPLEQUADROTOR_SIMPLEQUADROTOR_H_
diff --git a/include/scrimmage/plugins/motion/SingleIntegrator/SingleIntegrator.h b/include/scrimmage/plugins/motion/SingleIntegrator/SingleIntegrator.h
index 3c73499a6b..7663eb6699 100644
--- a/include/scrimmage/plugins/motion/SingleIntegrator/SingleIntegrator.h
+++ b/include/scrimmage/plugins/motion/SingleIntegrator/SingleIntegrator.h
@@ -42,8 +42,8 @@ namespace scrimmage {
 namespace motion {
 class SingleIntegrator : public scrimmage::MotionModel {
  public:
-    bool init(std::map<std::string, std::string> &info,
-              std::map<std::string, std::string> &params) override;
+    bool init(std::map<std::string, std::string>& info,
+              std::map<std::string, std::string>& params) override;
     bool step(double t, double dt) override;
 
  protected:
@@ -55,6 +55,6 @@ class SingleIntegrator : public scrimmage::MotionModel {
     uint8_t vel_z_idx_ = 0;
     uint8_t desired_heading_idx_ = 0;
 };
-} // namespace motion
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_MOTION_SINGLEINTEGRATOR_SINGLEINTEGRATOR_H_
+}  // namespace motion
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_MOTION_SINGLEINTEGRATOR_SINGLEINTEGRATOR_H_
diff --git a/include/scrimmage/plugins/motion/UUV6DOF/UUV6DOF.h b/include/scrimmage/plugins/motion/UUV6DOF/UUV6DOF.h
index 995b34b5e8..380ed49675 100644
--- a/include/scrimmage/plugins/motion/UUV6DOF/UUV6DOF.h
+++ b/include/scrimmage/plugins/motion/UUV6DOF/UUV6DOF.h
@@ -73,16 +73,16 @@ class UUV6DOF : public scrimmage::motion::RigidBody6DOFBase {
 
     UUV6DOF();
 
-    bool init(std::map<std::string, std::string> &info,
-              std::map<std::string, std::string> &params) override;
+    bool init(std::map<std::string, std::string>& info,
+              std::map<std::string, std::string>& params) override;
 
     bool step(double time, double dt) override;
 
-    void model(const vector_t &x, vector_t &dxdt, double t) override;
+    void model(const vector_t& x, vector_t& dxdt, double t) override;
 
     class Controller : public scrimmage::Controller {
      public:
-        virtual Eigen::Vector2d &u() = 0;
+        virtual Eigen::Vector2d& u() = 0;
     };
 
  protected:
diff --git a/include/scrimmage/plugins/motion/Unicycle/Unicycle.h b/include/scrimmage/plugins/motion/Unicycle/Unicycle.h
index ed75a86676..649d1c5746 100644
--- a/include/scrimmage/plugins/motion/Unicycle/Unicycle.h
+++ b/include/scrimmage/plugins/motion/Unicycle/Unicycle.h
@@ -42,12 +42,12 @@ namespace scrimmage {
 namespace motion {
 class Unicycle : public scrimmage::MotionModel {
  public:
-    bool init(std::map<std::string, std::string> &info,
-                      std::map<std::string, std::string> &params) override;
+    bool init(std::map<std::string, std::string>& info,
+              std::map<std::string, std::string>& params) override;
 
     bool step(double t, double dt) override;
 
-    void model(const vector_t &x , vector_t &dxdt , double t) override;
+    void model(const vector_t& x, vector_t& dxdt, double t) override;
 
  protected:
     double turn_rate_max_ = 1.0;
@@ -67,6 +67,6 @@ class Unicycle : public scrimmage::MotionModel {
     double pitch_rate_ = 0;
     double velocity_z_ = 0;
 };
-} // namespace motion
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_MOTION_UNICYCLE_UNICYCLE_H_
+}  // namespace motion
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_MOTION_UNICYCLE_UNICYCLE_H_
diff --git a/include/scrimmage/plugins/motion/Unicycle3D/Unicycle3D.h b/include/scrimmage/plugins/motion/Unicycle3D/Unicycle3D.h
index 8d8fe46f4b..f2076e6e34 100644
--- a/include/scrimmage/plugins/motion/Unicycle3D/Unicycle3D.h
+++ b/include/scrimmage/plugins/motion/Unicycle3D/Unicycle3D.h
@@ -33,24 +33,24 @@
 #ifndef INCLUDE_SCRIMMAGE_PLUGINS_MOTION_UNICYCLE3D_UNICYCLE3D_H_
 #define INCLUDE_SCRIMMAGE_PLUGINS_MOTION_UNICYCLE3D_UNICYCLE3D_H_
 
+#include <scrimmage/common/CSV.h>
+#include <scrimmage/common/PID.h>
 #include <scrimmage/math/State.h>
-#include <scrimmage/motion/MotionModel.h>
 #include <scrimmage/motion/Controller.h>
-#include <scrimmage/common/PID.h>
-#include <scrimmage/common/CSV.h>
+#include <scrimmage/motion/MotionModel.h>
 
+#include <limits>
 #include <map>
 #include <string>
-#include <limits>
 
 namespace scrimmage {
 namespace motion {
 class Unicycle3D : public scrimmage::MotionModel {
  public:
-    bool init(std::map<std::string, std::string> &info,
-              std::map<std::string, std::string> &params) override;
+    bool init(std::map<std::string, std::string>& info,
+              std::map<std::string, std::string>& params) override;
     bool step(double t, double dt) override;
-    void model(const vector_t &x , vector_t &dxdt , double t) override;
+    void model(const vector_t& x, vector_t& dxdt, double t) override;
 
  protected:
     double accel_max_ = -1.0;
@@ -85,6 +85,6 @@ class Unicycle3D : public scrimmage::MotionModel {
     double pitch_rate_ = 0;
     double roll_rate_ = 0;
 };
-} // namespace motion
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_MOTION_UNICYCLE3D_UNICYCLE3D_H_
+}  // namespace motion
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_MOTION_UNICYCLE3D_UNICYCLE3D_H_
diff --git a/include/scrimmage/plugins/network/GlobalNetwork/GlobalNetwork.h b/include/scrimmage/plugins/network/GlobalNetwork/GlobalNetwork.h
index 6755d051be..e38a9093c0 100644
--- a/include/scrimmage/plugins/network/GlobalNetwork/GlobalNetwork.h
+++ b/include/scrimmage/plugins/network/GlobalNetwork/GlobalNetwork.h
@@ -47,17 +47,19 @@ class GlobalNetwork : public scrimmage::Network {
  public:
     GlobalNetwork();
 
-    bool init(std::map<std::string, std::string> &mission_params,
-                      std::map<std::string, std::string> &plugin_params) override;
+    bool init(std::map<std::string, std::string>& mission_params,
+              std::map<std::string, std::string>& plugin_params) override;
+
  protected:
-    bool is_reachable(const scrimmage::EntityPluginPtr &pub_plugin,
-                              const scrimmage::EntityPluginPtr &sub_plugin) override;
+    bool is_reachable(const scrimmage::EntityPluginPtr& pub_plugin,
+                      const scrimmage::EntityPluginPtr& sub_plugin) override;
+
+    bool is_successful_transmission(const scrimmage::EntityPluginPtr& pub_plugin,
+                                    const scrimmage::EntityPluginPtr& sub_plugin) override;
 
-    bool is_successful_transmission(const scrimmage::EntityPluginPtr &pub_plugin,
-                                            const scrimmage::EntityPluginPtr &sub_plugin) override;
  protected:
  private:
 };
-} // namespace network
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_NETWORK_GLOBALNETWORK_GLOBALNETWORK_H_
+}  // namespace network
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_NETWORK_GLOBALNETWORK_GLOBALNETWORK_H_
diff --git a/include/scrimmage/plugins/network/LocalNetwork/LocalNetwork.h b/include/scrimmage/plugins/network/LocalNetwork/LocalNetwork.h
index 2e650e07f0..f4493353d3 100644
--- a/include/scrimmage/plugins/network/LocalNetwork/LocalNetwork.h
+++ b/include/scrimmage/plugins/network/LocalNetwork/LocalNetwork.h
@@ -47,17 +47,19 @@ class LocalNetwork : public scrimmage::Network {
  public:
     LocalNetwork();
 
-    bool init(std::map<std::string, std::string> &mission_params,
-                      std::map<std::string, std::string> &plugin_params) override;
+    bool init(std::map<std::string, std::string>& mission_params,
+              std::map<std::string, std::string>& plugin_params) override;
+
  protected:
-    bool is_reachable(const scrimmage::EntityPluginPtr &pub_plugin,
-                              const scrimmage::EntityPluginPtr &sub_plugin) override;
+    bool is_reachable(const scrimmage::EntityPluginPtr& pub_plugin,
+                      const scrimmage::EntityPluginPtr& sub_plugin) override;
+
+    bool is_successful_transmission(const scrimmage::EntityPluginPtr& pub_plugin,
+                                    const scrimmage::EntityPluginPtr& sub_plugin) override;
 
-    bool is_successful_transmission(const scrimmage::EntityPluginPtr &pub_plugin,
-                                            const scrimmage::EntityPluginPtr &sub_plugin) override;
  protected:
  private:
 };
-} // namespace network
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_NETWORK_LOCALNETWORK_LOCALNETWORK_H_
+}  // namespace network
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_NETWORK_LOCALNETWORK_LOCALNETWORK_H_
diff --git a/include/scrimmage/plugins/network/SphereNetwork/SphereNetwork.h b/include/scrimmage/plugins/network/SphereNetwork/SphereNetwork.h
index e11c45a065..b9877a08d4 100644
--- a/include/scrimmage/plugins/network/SphereNetwork/SphereNetwork.h
+++ b/include/scrimmage/plugins/network/SphereNetwork/SphereNetwork.h
@@ -43,14 +43,15 @@ namespace network {
 
 class SphereNetwork : public scrimmage::Network {
  public:
-    bool init(std::map<std::string, std::string> &mission_params,
-                      std::map<std::string, std::string> &plugin_params) override;
+    bool init(std::map<std::string, std::string>& mission_params,
+              std::map<std::string, std::string>& plugin_params) override;
+
  protected:
-    bool is_reachable(const scrimmage::EntityPluginPtr &pub_plugin,
-                              const scrimmage::EntityPluginPtr &sub_plugin) override;
+    bool is_reachable(const scrimmage::EntityPluginPtr& pub_plugin,
+                      const scrimmage::EntityPluginPtr& sub_plugin) override;
 
-    bool is_successful_transmission(const scrimmage::EntityPluginPtr &pub_plugin,
-                                            const scrimmage::EntityPluginPtr &sub_plugin) override;
+    bool is_successful_transmission(const scrimmage::EntityPluginPtr& pub_plugin,
+                                    const scrimmage::EntityPluginPtr& sub_plugin) override;
     double range_;
     double prob_transmit_;
 
@@ -64,6 +65,6 @@ class SphereNetwork : public scrimmage::Network {
     bool filter_comms_plane_ = false;
     bool within_planar_boundary(double z1, double z2);
 };
-} // namespace network
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_NETWORK_SPHERENETWORK_SPHERENETWORK_H_
+}  // namespace network
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_NETWORK_SPHERENETWORK_SPHERENETWORK_H_
diff --git a/include/scrimmage/plugins/sensor/AirSimSensor/AirSimSensor.h b/include/scrimmage/plugins/sensor/AirSimSensor/AirSimSensor.h
index 37bce8ec10..8d9acbac4d 100644
--- a/include/scrimmage/plugins/sensor/AirSimSensor/AirSimSensor.h
+++ b/include/scrimmage/plugins/sensor/AirSimSensor/AirSimSensor.h
@@ -32,29 +32,27 @@
 #ifndef INCLUDE_SCRIMMAGE_PLUGINS_SENSOR_AIRSIMSENSOR_AIRSIMSENSOR_H_
 #define INCLUDE_SCRIMMAGE_PLUGINS_SENSOR_AIRSIMSENSOR_AIRSIMSENSOR_H_
 
-#include <scrimmage/sensor/Sensor.h>
-#include <scrimmage/math/Angles.h>
 #include <scrimmage/common/CSV.h>
+#include <scrimmage/math/Angles.h>
+#include <scrimmage/sensor/Sensor.h>
 
-#include <random>
 #include <list>
 #include <map>
-#include <string>
-#include <vector>
 #include <memory>
-
 #include <opencv2/core/core.hpp>
 #include <opencv2/core/mat.hpp>
+#include <random>
+#include <string>
+#include <vector>
 
 // Eigen libraries
 #include "Eigen/Core"
 #include "Eigen/Geometry"
-
 #include "common/common_utils/StrictMode.hpp"
 STRICT_MODE_OFF
 #ifndef RPCLIB_MSGPACK
 #define RPCLIB_MSGPACK clmdep_msgpack
-#endif // !RPCLIB_MSGPACK
+#endif  // !RPCLIB_MSGPACK
 STRICT_MODE_ON
 
 #include "vehicles/multirotor/api/MultirotorRpcLibClient.hpp"
@@ -67,28 +65,27 @@ namespace scrimmage {
 namespace sensor {
 class CameraConfig {
  public:
-        msr::airlib::ImageCaptureBase::ImageType img_type =
-            msr::airlib::ImageCaptureBase::ImageType::Scene;
-
-        std::string vehicle_name = "none";
-        std::string cam_name = "none";
-        std::string img_type_name = "none";
-        int img_type_number = 0;
-        int height = 144; // 288
-        int width = 256;  // 512
-        int fov = 60;
-        bool pixels_as_float = false;
-
-        friend std::ostream& operator<<(std::ostream& os,
-                                        const CameraConfig& c) {
-            os << "Camera_Name=" << c.cam_name;
-            os << ", Image_Type_Num=" << c.img_type_number;
-            os << ", Image_Type_Name=" << c.img_type_name;
-            os << ", Height=" << c.height;
-            os << ", Width=" << c.width;
-            os << ", FOV=" << c.fov;
-            return os;
-        }
+    msr::airlib::ImageCaptureBase::ImageType img_type =
+        msr::airlib::ImageCaptureBase::ImageType::Scene;
+
+    std::string vehicle_name = "none";
+    std::string cam_name = "none";
+    std::string img_type_name = "none";
+    int img_type_number = 0;
+    int height = 144;  // 288
+    int width = 256;   // 512
+    int fov = 60;
+    bool pixels_as_float = false;
+
+    friend std::ostream& operator<<(std::ostream& os, const CameraConfig& c) {
+        os << "Camera_Name=" << c.cam_name;
+        os << ", Image_Type_Num=" << c.img_type_number;
+        os << ", Image_Type_Name=" << c.img_type_name;
+        os << ", Height=" << c.height;
+        os << ", Width=" << c.width;
+        os << ", FOV=" << c.fov;
+        return os;
+    }
 };
 
 class AirSimImageType {
@@ -126,18 +123,19 @@ class AirSimImuType {
 class AirSimSensor : public scrimmage::Sensor {
  public:
     AirSimSensor();
-    void init(std::map<std::string, std::string> &params) override;
+    void init(std::map<std::string, std::string>& params) override;
     bool step() override;
     void close(double t) override;
 
  protected:
     std::string vehicle_name_ = "none";
-    bool save_data(MessagePtr<std::vector<AirSimImageType>>& im_msg, StatePtr& state, int frame_num);
+    bool save_data(MessagePtr<std::vector<AirSimImageType>>& im_msg, StatePtr& state,
+                   int frame_num);
     scrimmage::CSV csv;
     int airsim_frame_num_ = 0;
 
     // Images
-    void parse_camera_configs(std::map<std::string, std::string> &params);
+    void parse_camera_configs(std::map<std::string, std::string>& params);
     std::list<CameraConfig> cam_configs_;
     std::thread request_images_thread_;
     void request_images();
@@ -149,7 +147,7 @@ class AirSimSensor : public scrimmage::Sensor {
     std::mutex new_image_mutex_;
 
     // LIDAR
-    void parse_lidar_configs(std::map<std::string, std::string> &params);
+    void parse_lidar_configs(std::map<std::string, std::string>& params);
     std::vector<std::string> lidar_names_;
     std::thread request_lidar_thread_;
     void request_lidar();
@@ -161,7 +159,7 @@ class AirSimSensor : public scrimmage::Sensor {
     std::mutex new_lidar_mutex_;
 
     // IMU
-    void parse_imu_configs(std::map<std::string, std::string> &params);
+    void parse_imu_configs(std::map<std::string, std::string>& params);
     std::vector<std::string> imu_names_;
     std::thread request_imu_thread_;
     void request_imu();
@@ -197,6 +195,6 @@ class AirSimSensor : public scrimmage::Sensor {
 
  private:
 };
-} // namespace sensor
-} // namespace scrimmage
+}  // namespace sensor
+}  // namespace scrimmage
 #endif  // INCLUDE_SCRIMMAGE_PLUGINS_SENSOR_AIRSIMSENSOR_AIRSIMSENSOR_H_
diff --git a/include/scrimmage/plugins/sensor/AltitudeAboveTerrain/AltitudeAboveTerrain.h b/include/scrimmage/plugins/sensor/AltitudeAboveTerrain/AltitudeAboveTerrain.h
index 8a906a9bb7..326a4b31ac 100644
--- a/include/scrimmage/plugins/sensor/AltitudeAboveTerrain/AltitudeAboveTerrain.h
+++ b/include/scrimmage/plugins/sensor/AltitudeAboveTerrain/AltitudeAboveTerrain.h
@@ -33,24 +33,24 @@
 #ifndef INCLUDE_SCRIMMAGE_PLUGINS_SENSOR_ALTITUDEABOVETERRAIN_ALTITUDEABOVETERRAIN_H_
 #define INCLUDE_SCRIMMAGE_PLUGINS_SENSOR_ALTITUDEABOVETERRAIN_ALTITUDEABOVETERRAIN_H_
 
-#include <scrimmage/sensor/Sensor.h>
-#include <scrimmage/entity/Entity.h>
 #include <scrimmage/entity/Contact.h>
-#include <scrimmage/pubsub/Publisher.h>
+#include <scrimmage/entity/Entity.h>
 #include <scrimmage/plugins/interaction/TerrainGenerator/TerrainMap.h>
+#include <scrimmage/pubsub/Publisher.h>
+#include <scrimmage/sensor/Sensor.h>
 
-#include <random>
-#include <vector>
 #include <map>
-#include <string>
 #include <memory>
+#include <random>
+#include <string>
+#include <vector>
 
 namespace scrimmage {
 namespace sensor {
 class AltitudeAboveTerrain : public scrimmage::Sensor {
  public:
     AltitudeAboveTerrain();
-    void init(std::map<std::string, std::string> &params) override;
+    void init(std::map<std::string, std::string>& params) override;
     bool step() override;
 
  protected:
@@ -59,8 +59,9 @@ class AltitudeAboveTerrain : public scrimmage::Sensor {
     PublisherPtr pub_true_;
     PublisherPtr pub_noise_;
     std::shared_ptr<scrimmage::interaction::TerrainMap> map_ = nullptr;
+
  private:
 };
-} // namespace sensor
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_SENSOR_ALTITUDEABOVETERRAIN_ALTITUDEABOVETERRAIN_H_
+}  // namespace sensor
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_SENSOR_ALTITUDEABOVETERRAIN_ALTITUDEABOVETERRAIN_H_
diff --git a/include/scrimmage/plugins/sensor/ContactBlobCamera/ContactBlobCamera.h b/include/scrimmage/plugins/sensor/ContactBlobCamera/ContactBlobCamera.h
index 6ca6a0ef6f..b6716bc88b 100644
--- a/include/scrimmage/plugins/sensor/ContactBlobCamera/ContactBlobCamera.h
+++ b/include/scrimmage/plugins/sensor/ContactBlobCamera/ContactBlobCamera.h
@@ -33,19 +33,17 @@
 #ifndef INCLUDE_SCRIMMAGE_PLUGINS_SENSOR_CONTACTBLOBCAMERA_CONTACTBLOBCAMERA_H_
 #define INCLUDE_SCRIMMAGE_PLUGINS_SENSOR_CONTACTBLOBCAMERA_CONTACTBLOBCAMERA_H_
 
-#include <Eigen/Dense>
-
-#include <scrimmage/sensor/Sensor.h>
 #include <scrimmage/entity/Contact.h>
+#include <scrimmage/sensor/Sensor.h>
 
-#include <random>
-#include <map>
-#include <string>
-#include <vector>
+#include <Eigen/Dense>
 #include <fstream>
+#include <map>
 #include <memory>
-
 #include <opencv2/core/core.hpp>
+#include <random>
+#include <string>
+#include <vector>
 
 namespace scrimmage {
 
@@ -54,7 +52,7 @@ class ContactBlobCameraType;
 
 class ContactBlobCamera : public scrimmage::Sensor {
  public:
-    void init(std::map<std::string, std::string> &params) override;
+    void init(std::map<std::string, std::string>& params) override;
     bool step() override;
 
  protected:
@@ -66,21 +64,18 @@ class ContactBlobCamera : public scrimmage::Sensor {
     std::vector<std::shared_ptr<std::normal_distribution<double>>> size_noise_;
 
     void contacts_to_bounding_boxes(
-        const scrimmage::State &sensor_frame,
-        scrimmage::ContactMap &contacts,
-        std::shared_ptr<scrimmage::Message<ContactBlobCameraType>> &msg);
+        const scrimmage::State& sensor_frame, scrimmage::ContactMap& contacts,
+        std::shared_ptr<scrimmage::Message<ContactBlobCameraType>>& msg);
 
-    void add_false_positives(
-        std::shared_ptr<scrimmage::Message<ContactBlobCameraType>> &msg);
+    void add_false_positives(std::shared_ptr<scrimmage::Message<ContactBlobCameraType>>& msg);
 
     Eigen::Vector2d project_rel_3d_to_2d(Eigen::Vector3d rel_pos);
     bool in_field_of_view(Eigen::Vector3d rel_pos);
-    void draw_object_with_bounding_box(cv::Mat &frame, const int &id,
-                                       const cv::Rect &rect,
-                                       const Eigen::Vector2d &center,
-                                       const double &radius);
+    void draw_object_with_bounding_box(cv::Mat& frame, const int& id, const cv::Rect& rect,
+                                       const Eigen::Vector2d& center, const double& radius);
     void set_plugin_params(std::map<std::string, double> params);
-    void draw_frustum(const std::vector<scrimmage_proto::ShapePtr>& frustum, double x_rot, double y_rot, double z_rot);
+    void draw_frustum(const std::vector<scrimmage_proto::ShapePtr>& frustum, double x_rot,
+                      double y_rot, double z_rot);
 
     // plugin parameters
     std::map<std::string, double> plugin_params_;
@@ -112,7 +107,7 @@ class ContactBlobCamera : public scrimmage::Sensor {
     bool show_sim_contacts_ = false;
 
     double last_contact_send_time_ = 0.0;
-    double contact_send_dt_ = 1.0; // seconds
+    double contact_send_dt_ = 1.0;  // seconds
 
     std::string window_name_ = "ContactBlobCamera";
     std::vector<scrimmage_proto::ShapePtr> frustum_shapes_;
@@ -120,6 +115,6 @@ class ContactBlobCamera : public scrimmage::Sensor {
 
     scrimmage::ContactMap sim_contacts_;
 };
-} // namespace sensor
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_SENSOR_CONTACTBLOBCAMERA_CONTACTBLOBCAMERA_H_
+}  // namespace sensor
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_SENSOR_CONTACTBLOBCAMERA_CONTACTBLOBCAMERA_H_
diff --git a/include/scrimmage/plugins/sensor/ContactBlobCamera/ContactBlobCameraType.h b/include/scrimmage/plugins/sensor/ContactBlobCamera/ContactBlobCameraType.h
index fabb2c0366..fd02b87ba4 100644
--- a/include/scrimmage/plugins/sensor/ContactBlobCamera/ContactBlobCameraType.h
+++ b/include/scrimmage/plugins/sensor/ContactBlobCamera/ContactBlobCameraType.h
@@ -34,8 +34,8 @@
 #define INCLUDE_SCRIMMAGE_PLUGINS_SENSOR_CONTACTBLOBCAMERA_CONTACTBLOBCAMERATYPE_H_
 
 #include <map>
-#include <vector>
 #include <opencv2/core/core.hpp>
+#include <vector>
 
 namespace scrimmage {
 namespace sensor {
@@ -58,6 +58,6 @@ class ContactBlobCameraType {
     double pitch;
     double yaw;
 };
-} // namespace sensor
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_SENSOR_CONTACTBLOBCAMERA_CONTACTBLOBCAMERATYPE_H_
+}  // namespace sensor
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_SENSOR_CONTACTBLOBCAMERA_CONTACTBLOBCAMERATYPE_H_
diff --git a/include/scrimmage/plugins/sensor/GPS/GPS.h b/include/scrimmage/plugins/sensor/GPS/GPS.h
index b7864be631..597c1f72fd 100644
--- a/include/scrimmage/plugins/sensor/GPS/GPS.h
+++ b/include/scrimmage/plugins/sensor/GPS/GPS.h
@@ -35,11 +35,11 @@
 
 #include <scrimmage/sensor/Sensor.h>
 
-#include <random>
-#include <vector>
 #include <map>
-#include <string>
 #include <memory>
+#include <random>
+#include <string>
+#include <vector>
 
 namespace scrimmage {
 
@@ -51,7 +51,7 @@ namespace sensor {
 class GPS : public scrimmage::Sensor {
  public:
     GPS();
-    void init(std::map<std::string, std::string> &params) override;
+    void init(std::map<std::string, std::string>& params) override;
     bool step() override;
 
  protected:
@@ -60,8 +60,9 @@ class GPS : public scrimmage::Sensor {
     std::vector<int> gps_denied_ids_;
     PublisherPtr pub_;
     std::shared_ptr<scrimmage::interaction::BoundaryBase> boundary_ = NULL;
+
  private:
 };
-} // namespace sensor
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_SENSOR_GPS_GPS_H_
+}  // namespace sensor
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_SENSOR_GPS_GPS_H_
diff --git a/include/scrimmage/plugins/sensor/LOSSensor/LOSSensor.h b/include/scrimmage/plugins/sensor/LOSSensor/LOSSensor.h
index 85eb7962de..5bb0752478 100644
--- a/include/scrimmage/plugins/sensor/LOSSensor/LOSSensor.h
+++ b/include/scrimmage/plugins/sensor/LOSSensor/LOSSensor.h
@@ -35,19 +35,18 @@
 #include <scrimmage/plugins/sensor/RayTrace/RayTrace.h>
 
 #include <Eigen/Dense>
-
 #include <map>
+#include <memory>
+#include <random>
 #include <string>
 #include <vector>
-#include <random>
-#include <memory>
 
 namespace scrimmage {
 namespace sensor {
 class LOSSensor : public RayTrace {
  public:
     LOSSensor();
-    void init(std::map<std::string, std::string> &params) override;
+    void init(std::map<std::string, std::string>& params) override;
     bool step() override;
 
  private:
diff --git a/include/scrimmage/plugins/sensor/NoisyContacts/NoisyContacts.h b/include/scrimmage/plugins/sensor/NoisyContacts/NoisyContacts.h
index 32ab542137..f11d100c92 100644
--- a/include/scrimmage/plugins/sensor/NoisyContacts/NoisyContacts.h
+++ b/include/scrimmage/plugins/sensor/NoisyContacts/NoisyContacts.h
@@ -35,18 +35,18 @@
 
 #include <scrimmage/sensor/Sensor.h>
 
-#include <random>
-#include <vector>
 #include <map>
-#include <string>
 #include <memory>
+#include <random>
+#include <string>
+#include <vector>
 
 namespace scrimmage {
 namespace sensor {
 class NoisyContacts : public scrimmage::Sensor {
  public:
     NoisyContacts();
-    void init(std::map<std::string, std::string> &params) override;
+    void init(std::map<std::string, std::string>& params) override;
     bool step() override;
 
  protected:
@@ -54,8 +54,9 @@ class NoisyContacts : public scrimmage::Sensor {
     std::vector<std::shared_ptr<std::normal_distribution<double>>> vel_noise_;
     std::vector<std::shared_ptr<std::normal_distribution<double>>> orient_noise_;
     PublisherPtr pub_;
+
  private:
 };
-} // namespace sensor
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_SENSOR_NOISYCONTACTS_NOISYCONTACTS_H_
+}  // namespace sensor
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_SENSOR_NOISYCONTACTS_NOISYCONTACTS_H_
diff --git a/include/scrimmage/plugins/sensor/NoisyState/NoisyState.h b/include/scrimmage/plugins/sensor/NoisyState/NoisyState.h
index 350e669fd4..ad6bfc09ef 100644
--- a/include/scrimmage/plugins/sensor/NoisyState/NoisyState.h
+++ b/include/scrimmage/plugins/sensor/NoisyState/NoisyState.h
@@ -36,15 +36,15 @@
 #include <scrimmage/sensor/Sensor.h>
 
 #include <map>
+#include <memory>
 #include <string>
 #include <vector>
-#include <memory>
 
 namespace scrimmage {
 namespace sensor {
 class NoisyState : public scrimmage::Sensor {
  public:
-    void init(std::map<std::string, std::string> &params) override;
+    void init(std::map<std::string, std::string>& params) override;
     bool step() override;
 
  protected:
@@ -54,6 +54,6 @@ class NoisyState : public scrimmage::Sensor {
 
     PublisherPtr pub_;
 };
-} // namespace sensor
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_SENSOR_NOISYSTATE_NOISYSTATE_H_
+}  // namespace sensor
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_SENSOR_NOISYSTATE_NOISYSTATE_H_
diff --git a/include/scrimmage/plugins/sensor/RLConsensusSensor/RLConsensusSensor.h b/include/scrimmage/plugins/sensor/RLConsensusSensor/RLConsensusSensor.h
index 91a44f4021..3929f934c8 100644
--- a/include/scrimmage/plugins/sensor/RLConsensusSensor/RLConsensusSensor.h
+++ b/include/scrimmage/plugins/sensor/RLConsensusSensor/RLConsensusSensor.h
@@ -45,9 +45,9 @@ class RLConsensusSensor : public ScrimmageOpenAISensor {
  public:
     RLConsensusSensor();
     void set_observation_space() override;
-    void get_observation(double *data, uint32_t beg_idx, uint32_t end_idx) override;
+    void get_observation(double* data, uint32_t beg_idx, uint32_t end_idx) override;
 };
-} // namespace sensor
-} // namespace scrimmage
+}  // namespace sensor
+}  // namespace scrimmage
 
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_SENSOR_RLCONSENSUSSENSOR_RLCONSENSUSSENSOR_H_
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_SENSOR_RLCONSENSUSSENSOR_RLCONSENSUSSENSOR_H_
diff --git a/include/scrimmage/plugins/sensor/RLSimpleSensor/RLSimpleSensor.h b/include/scrimmage/plugins/sensor/RLSimpleSensor/RLSimpleSensor.h
index 2ad120556c..3a526e3ce4 100644
--- a/include/scrimmage/plugins/sensor/RLSimpleSensor/RLSimpleSensor.h
+++ b/include/scrimmage/plugins/sensor/RLSimpleSensor/RLSimpleSensor.h
@@ -45,10 +45,10 @@ namespace sensor {
 class RLSimpleSensor : public ScrimmageOpenAISensor {
  public:
     void set_observation_space() override;
-    void get_observation(double *data, uint32_t beg_idx, uint32_t end_idx) override;
+    void get_observation(double* data, uint32_t beg_idx, uint32_t end_idx) override;
 };
 
-} // namespace sensor
-} // namespace scrimmage
+}  // namespace sensor
+}  // namespace scrimmage
 
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_SENSOR_RLSIMPLESENSOR_RLSIMPLESENSOR_H_
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_SENSOR_RLSIMPLESENSOR_RLSIMPLESENSOR_H_
diff --git a/include/scrimmage/plugins/sensor/ROSAltimeter/ROSAltimeter.h b/include/scrimmage/plugins/sensor/ROSAltimeter/ROSAltimeter.h
index 05e88f8a4b..5973319395 100644
--- a/include/scrimmage/plugins/sensor/ROSAltimeter/ROSAltimeter.h
+++ b/include/scrimmage/plugins/sensor/ROSAltimeter/ROSAltimeter.h
@@ -32,17 +32,16 @@
 #ifndef INCLUDE_SCRIMMAGE_PLUGINS_SENSOR_ROSALTIMETER_ROSALTIMETER_H_
 #define INCLUDE_SCRIMMAGE_PLUGINS_SENSOR_ROSALTIMETER_ROSALTIMETER_H_
 
-#include <ros/ros.h>
 #include <mavros_msgs/Altitude.h>
+#include <ros/ros.h>
 #include <scrimmage/sensor/Sensor.h>
 
-#include <random>
-#include <vector>
+#include <GeographicLib/LocalCartesian.hpp>
 #include <map>
-#include <string>
 #include <memory>
-
-#include <GeographicLib/LocalCartesian.hpp>
+#include <random>
+#include <string>
+#include <vector>
 
 namespace scrimmage {
 namespace sensor {
@@ -50,7 +49,7 @@ namespace sensor {
 class ROSAltimeter : public scrimmage::Sensor {
  public:
     ROSAltimeter();
-    void init(std::map<std::string, std::string> &params) override;
+    void init(std::map<std::string, std::string>& params) override;
     bool step() override;
 
  protected:
@@ -62,6 +61,6 @@ class ROSAltimeter : public scrimmage::Sensor {
 
  private:
 };
-} // namespace sensor
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_SENSOR_ROSALTIMETER_ROSALTIMETER_H_
+}  // namespace sensor
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_SENSOR_ROSALTIMETER_ROSALTIMETER_H_
diff --git a/include/scrimmage/plugins/sensor/ROSCompass/ROSCompass.h b/include/scrimmage/plugins/sensor/ROSCompass/ROSCompass.h
index 5363f34fa3..233fe3c67e 100644
--- a/include/scrimmage/plugins/sensor/ROSCompass/ROSCompass.h
+++ b/include/scrimmage/plugins/sensor/ROSCompass/ROSCompass.h
@@ -33,14 +33,14 @@
 #define INCLUDE_SCRIMMAGE_PLUGINS_SENSOR_ROSCOMPASS_ROSCOMPASS_H_
 
 #include <ros/ros.h>
-#include <sensor_msgs/MagneticField.h>
 #include <scrimmage/sensor/Sensor.h>
+#include <sensor_msgs/MagneticField.h>
 
-#include <random>
-#include <vector>
 #include <map>
-#include <string>
 #include <memory>
+#include <random>
+#include <string>
+#include <vector>
 
 namespace scrimmage {
 namespace sensor {
@@ -48,7 +48,7 @@ namespace sensor {
 class ROSCompass : public scrimmage::Sensor {
  public:
     ROSCompass();
-    void init(std::map<std::string, std::string> &params) override;
+    void init(std::map<std::string, std::string>& params) override;
     bool step() override;
 
  protected:
@@ -59,6 +59,6 @@ class ROSCompass : public scrimmage::Sensor {
 
  private:
 };
-} // namespace sensor
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_SENSOR_ROSCOMPASS_ROSCOMPASS_H_
+}  // namespace sensor
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_SENSOR_ROSCOMPASS_ROSCOMPASS_H_
diff --git a/include/scrimmage/plugins/sensor/ROSIMUSensor/HG4930IMUBudget.h b/include/scrimmage/plugins/sensor/ROSIMUSensor/HG4930IMUBudget.h
index 49d790da00..0ec7f8e7b1 100644
--- a/include/scrimmage/plugins/sensor/ROSIMUSensor/HG4930IMUBudget.h
+++ b/include/scrimmage/plugins/sensor/ROSIMUSensor/HG4930IMUBudget.h
@@ -37,33 +37,36 @@
 
 class HG4930IMUBudget : public IMUErrorBudgetTemplate {
  public:
-    bool localInRunBiasOption = true; // false = 1st order Gauss-Markov process, true = Barnes-Jarvis model
+    bool localInRunBiasOption =
+        true;  // false = 1st order Gauss-Markov process, true = Barnes-Jarvis model
 
     // Accelerometer Spec.'s
-    double AccelFullScaleG = 20; // +/- 30 g
+    double AccelFullScaleG = 20;  // +/- 30 g
     // Scale factor & misalignment
-    double AccelScaleFactorPPMStdDev = 600.0;   // scale factor 1-sigma in ppm
-    double AccelMisalignmentRadians = 0.0 * 1e-6; // 100 micro-rad 1-sigma
+    double AccelScaleFactorPPMStdDev = 600.0;      // scale factor 1-sigma in ppm
+    double AccelMisalignmentRadians = 0.0 * 1e-6;  // 100 micro-rad 1-sigma
     // Bias
-    double AccelBiasRepeatabilityG = 1.7e-3; // 1 milli-g 1-sigma
+    double AccelBiasRepeatabilityG = 1.7e-3;  // 1 milli-g 1-sigma
     double AccelBiasInstabilityG = 0.025e-3;  // 50 micro-g 1-sigma
-    double AccelBiasTimeConstantSec = 60.0;  // accel bias time constant in seconds
+    double AccelBiasTimeConstantSec = 60.0;   // accel bias time constant in seconds
     // Velocity random walk
-    double AccelVRWGPerRtHz = 0.03 * (1.0 / 60) * 1.0 / 9.8; // Units of g's per root hertz -  0.065 fps/rt(hr)
+    double AccelVRWGPerRtHz =
+        0.03 * (1.0 / 60) * 1.0 / 9.8;  // Units of g's per root hertz -  0.065 fps/rt(hr)
     // Quantization
     double localAccelDeltaVQuantizationMPS = pow(2, -29);
 
     // Gyro Spec.'s
-    double GyroFullScaleDegPerSec = 400.0; // Full scale in degrees/second +/- 1000
+    double GyroFullScaleDegPerSec = 400.0;  // Full scale in degrees/second +/- 1000
     // Scale factor & misalignment
-    double GyroScaleFactorPPMStdDev = 600;     // 0.01% (i.e. 150 PPM) 1-sigma
-    double GyroMisalignmentRadians = 0.0 * 1e-6; // 100 arcsec 1-sigma
+    double GyroScaleFactorPPMStdDev = 600;        // 0.01% (i.e. 150 PPM) 1-sigma
+    double GyroMisalignmentRadians = 0.0 * 1e-6;  // 100 arcsec 1-sigma
     // Bias
-    double GyroBiasRepeatabilityDegPerHr = 7.0;       // 1.0 deg/Hr 1-sigma
-    double GyroBiasInstabilityDegPerHr = 0.25;         // 1 deg/Hr 1-sigma
-    double GyroBiasInstabilityTimeConstantSec = 60.0; // 100.0 second time constant for in-run bias instability
+    double GyroBiasRepeatabilityDegPerHr = 7.0;  // 1.0 deg/Hr 1-sigma
+    double GyroBiasInstabilityDegPerHr = 0.25;   // 1 deg/Hr 1-sigma
+    double GyroBiasInstabilityTimeConstantSec =
+        60.0;  // 100.0 second time constant for in-run bias instability
     // Angle random walk
-    double GyroARWDegPerRtHr = 0.04; // 0.09 deg/sqrt(hr)
+    double GyroARWDegPerRtHr = 0.04;  // 0.09 deg/sqrt(hr)
     // Quantization
     double localGyroDeltaThetaQuantizationRadians = pow(2, -33);
 
@@ -71,4 +74,4 @@ class HG4930IMUBudget : public IMUErrorBudgetTemplate {
     void CalculateParameters();
 };
 
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_SENSOR_ROSIMUSENSOR_HG4930IMUBUDGET_H_
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_SENSOR_ROSIMUSENSOR_HG4930IMUBUDGET_H_
diff --git a/include/scrimmage/plugins/sensor/ROSIMUSensor/IMUErrorBudgetTemplate.h b/include/scrimmage/plugins/sensor/ROSIMUSensor/IMUErrorBudgetTemplate.h
index 843b0c848e..71d7e8b4f0 100644
--- a/include/scrimmage/plugins/sensor/ROSIMUSensor/IMUErrorBudgetTemplate.h
+++ b/include/scrimmage/plugins/sensor/ROSIMUSensor/IMUErrorBudgetTemplate.h
@@ -35,70 +35,94 @@
 class IMUErrorBudgetTemplate {
  public:
     double SampleFrequency;
-    bool InRunBiasOption; // 0 = 1st order Gauss-Markov process, 1 = Barnes-Jarvis model
+    bool InRunBiasOption;  // 0 = 1st order Gauss-Markov process, 1 = Barnes-Jarvis model
     // Accelerometer Spec.'s
     // Scale factor
-    double XAccelAxisScaleFactorPPMStdDev = 0.0; // 1-sigma in units of PPM
-    double YAccelAxisScaleFactorPPMStdDev = 0.0; // 1-sigma in units of PPM
-    double ZAccelAxisScaleFactorPPMStdDev = 0.0; // 1-sigma in units of PPM
+    double XAccelAxisScaleFactorPPMStdDev = 0.0;  // 1-sigma in units of PPM
+    double YAccelAxisScaleFactorPPMStdDev = 0.0;  // 1-sigma in units of PPM
+    double ZAccelAxisScaleFactorPPMStdDev = 0.0;  // 1-sigma in units of PPM
     // Scale factor Nonlinearity
-    double XAccelAxisScaleFactorNonlinearityMPS2PerMPS22StdDev = 0.0; // 1-sigma in units of PPM
-    double YAccelAxisScaleFactorNonlinearityMPS2PerMPS22StdDev = 0.0; // 1-sigma in units of PPM
-    double ZAccelAxisScaleFactorNonlinearityMPS2PerMPS22StdDev = 0.0; // 1-sigma in units of PPM
+    double XAccelAxisScaleFactorNonlinearityMPS2PerMPS22StdDev = 0.0;  // 1-sigma in units of PPM
+    double YAccelAxisScaleFactorNonlinearityMPS2PerMPS22StdDev = 0.0;  // 1-sigma in units of PPM
+    double ZAccelAxisScaleFactorNonlinearityMPS2PerMPS22StdDev = 0.0;  // 1-sigma in units of PPM
     // Misalignment
-    double AccelMisalignmentPPMStdDev = 0.0; // 1-sigma in units of PPM
+    double AccelMisalignmentPPMStdDev = 0.0;  // 1-sigma in units of PPM
     // Turn-on Bias
-    double XAccelAxisTurnOnBiasMPS2StdDev = 0.0; // 1-sigma in units of meters per second (MPS) squared
-    double YAccelAxisTurnOnBiasMPS2StdDev = 0.0; // 1-sigma in units of meters per second (MPS) squared
-    double ZAccelAxisTurnOnBiasMPS2StdDev = 0.0; // 1-sigma in units of meters per second (MPS) squared
+    double XAccelAxisTurnOnBiasMPS2StdDev =
+        0.0;  // 1-sigma in units of meters per second (MPS) squared
+    double YAccelAxisTurnOnBiasMPS2StdDev =
+        0.0;  // 1-sigma in units of meters per second (MPS) squared
+    double ZAccelAxisTurnOnBiasMPS2StdDev =
+        0.0;  // 1-sigma in units of meters per second (MPS) squared
     // In-run Bias
-    double XAccelAxisInRunBiasMPS2StdDev = 0.0; // 1-sigma in units of meters per second (MPS) squared
-    double YAccelAxisInRunBiasMPS2StdDev = 0.0; // 1-sigma in units of meters per second (MPS) squared
-    double ZAccelAxisInRunBiasMPS2StdDev = 0.0; // 1-sigma in units of meters per second (MPS) squared
+    double XAccelAxisInRunBiasMPS2StdDev =
+        0.0;  // 1-sigma in units of meters per second (MPS) squared
+    double YAccelAxisInRunBiasMPS2StdDev =
+        0.0;  // 1-sigma in units of meters per second (MPS) squared
+    double ZAccelAxisInRunBiasMPS2StdDev =
+        0.0;  // 1-sigma in units of meters per second (MPS) squared
 
-    double XAccelAxisInRunBiasTimeConstantSec = 100.0; // in-run bias time constant in units of seconds
-    double YAccelAxisInRunBiasTimeConstantSec = 100.0; // in-run bias time constant in units of seconds
-    double ZAccelAxisInRunBiasTimeConstantSec = 100.0; // in-run bias time constant in units of seconds
+    double XAccelAxisInRunBiasTimeConstantSec =
+        100.0;  // in-run bias time constant in units of seconds
+    double YAccelAxisInRunBiasTimeConstantSec =
+        100.0;  // in-run bias time constant in units of seconds
+    double ZAccelAxisInRunBiasTimeConstantSec =
+        100.0;  // in-run bias time constant in units of seconds
 
-    double XAccelAxisInRunBiasContinuousProcessNoiseVar = 0.0; // variance of continuous-time process noise driving in-run bias
-    double YAccelAxisInRunBiasContinuousProcessNoiseVar = 0.0; // variance of continuous-time process noise driving in-run bias
-    double ZAccelAxisInRunBiasContinuousProcessNoiseVar = 0.0; // variance of continuous-time process noise driving in-run bias
+    double XAccelAxisInRunBiasContinuousProcessNoiseVar =
+        0.0;  // variance of continuous-time process noise driving in-run bias
+    double YAccelAxisInRunBiasContinuousProcessNoiseVar =
+        0.0;  // variance of continuous-time process noise driving in-run bias
+    double ZAccelAxisInRunBiasContinuousProcessNoiseVar =
+        0.0;  // variance of continuous-time process noise driving in-run bias
     // Velocity random walk
-    double XAccelVRWMPS2PerRtHzStdDev = 0.0; // velocity random walk (VRW) 1-sigma in units of meters per second (MPS) squared
-    double YAccelVRWMPS2PerRtHzStdDev = 0.0; // velocity random walk (VRW) 1-sigma in units of meters per second (MPS) squared
-    double ZAccelVRWMPS2PerRtHzStdDev = 0.0; // velocity random walk (VRW) 1-sigma in units of meters per second (MPS) squared
+    double XAccelVRWMPS2PerRtHzStdDev =
+        0.0;  // velocity random walk (VRW) 1-sigma in units of meters per second (MPS) squared
+    double YAccelVRWMPS2PerRtHzStdDev =
+        0.0;  // velocity random walk (VRW) 1-sigma in units of meters per second (MPS) squared
+    double ZAccelVRWMPS2PerRtHzStdDev =
+        0.0;  // velocity random walk (VRW) 1-sigma in units of meters per second (MPS) squared
     // Quantization
     double AccelDeltaVQuantizationMPS;
 
     // Gyro Spec.'s
     // Scale factor
-    double XGyroAxisScaleFactorPPMStdDev = 0.0; // 1-sigma in units of PPM
-    double YGyroAxisScaleFactorPPMStdDev = 0.0; // 1-sigma in units of PPM
-    double ZGyroAxisScaleFactorPPMStdDev = 0.0; // 1-sigma in units of PPM
+    double XGyroAxisScaleFactorPPMStdDev = 0.0;  // 1-sigma in units of PPM
+    double YGyroAxisScaleFactorPPMStdDev = 0.0;  // 1-sigma in units of PPM
+    double ZGyroAxisScaleFactorPPMStdDev = 0.0;  // 1-sigma in units of PPM
     // Misalignment
-    double GyroMisalignmentPPMStdDev = 0.0; // 1-sigma in units of PPM
+    double GyroMisalignmentPPMStdDev = 0.0;  // 1-sigma in units of PPM
     // Turn-on bias
-    double XGyroAxisTurnOnBiasRadPerSecStdDev = 0.0; // 1-sigma in units of radians/second
-    double YGyroAxisTurnOnBiasRadPerSecStdDev = 0.0; // 1-sigma in units of radians/second
-    double ZGyroAxisTurnOnBiasRadPerSecStdDev = 0.0; // 1-sigma in units of radians/second
+    double XGyroAxisTurnOnBiasRadPerSecStdDev = 0.0;  // 1-sigma in units of radians/second
+    double YGyroAxisTurnOnBiasRadPerSecStdDev = 0.0;  // 1-sigma in units of radians/second
+    double ZGyroAxisTurnOnBiasRadPerSecStdDev = 0.0;  // 1-sigma in units of radians/second
     // In-run bias
-    double XGyroAxisInRunBiasRadPerSecStdDev = 0.0; // 1-sigma in units of radians/second
-    double YGyroAxisInRunBiasRadPerSecStdDev = 0.0; // 1-sigma in units of radians/second
-    double ZGyroAxisInRunBiasRadPerSecStdDev = 0.0; // 1-sigma in units of radians/second
+    double XGyroAxisInRunBiasRadPerSecStdDev = 0.0;  // 1-sigma in units of radians/second
+    double YGyroAxisInRunBiasRadPerSecStdDev = 0.0;  // 1-sigma in units of radians/second
+    double ZGyroAxisInRunBiasRadPerSecStdDev = 0.0;  // 1-sigma in units of radians/second
 
-    double XGyroAxisInRunBiasTimeConstantSec = 300.0; // in-run bias time constant in units of seconds
-    double YGyroAxisInRunBiasTimeConstantSec = 300.0; // in-run bias time constant in units of seconds
-    double ZGyroAxisInRunBiasTimeConstantSec = 300.0; // in-run bias time constant in units of seconds
+    double XGyroAxisInRunBiasTimeConstantSec =
+        300.0;  // in-run bias time constant in units of seconds
+    double YGyroAxisInRunBiasTimeConstantSec =
+        300.0;  // in-run bias time constant in units of seconds
+    double ZGyroAxisInRunBiasTimeConstantSec =
+        300.0;  // in-run bias time constant in units of seconds
 
-    double XGyroAxisInRunBiasContinuousProcessNoiseVar = 0.0; // variance of continuous-time process noise driving in-run bias
-    double YGyroAxisInRunBiasContinuousProcessNoiseVar = 0.0; // variance of continuous-time process noise driving in-run bias
-    double ZGyroAxisInRunBiasContinuousProcessNoiseVar = 0.0; // variance of continuous-time process noise driving in-run bias
+    double XGyroAxisInRunBiasContinuousProcessNoiseVar =
+        0.0;  // variance of continuous-time process noise driving in-run bias
+    double YGyroAxisInRunBiasContinuousProcessNoiseVar =
+        0.0;  // variance of continuous-time process noise driving in-run bias
+    double ZGyroAxisInRunBiasContinuousProcessNoiseVar =
+        0.0;  // variance of continuous-time process noise driving in-run bias
     // Angle random walk
-    double XGyroARWRadPerSecPerRtHzStdDev = 0.0; // angular random walk (ARW) 1-sigma in units of radians/sec
-    double YGyroARWRadPerSecPerRtHzStdDev = 0.0; // angular random walk (ARW) 1-sigma in units of radians/sec
-    double ZGyroARWRadPerSecPerRtHzStdDev = 0.0; // angular random walk (ARW) 1-sigma in units of radians/sec
+    double XGyroARWRadPerSecPerRtHzStdDev =
+        0.0;  // angular random walk (ARW) 1-sigma in units of radians/sec
+    double YGyroARWRadPerSecPerRtHzStdDev =
+        0.0;  // angular random walk (ARW) 1-sigma in units of radians/sec
+    double ZGyroARWRadPerSecPerRtHzStdDev =
+        0.0;  // angular random walk (ARW) 1-sigma in units of radians/sec
     // Quantization
     double GyroDeltaThetaQuantizationRadians;
 };
 
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_SENSOR_ROSIMUSENSOR_IMUERRORBUDGETTEMPLATE_H_
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_SENSOR_ROSIMUSENSOR_IMUERRORBUDGETTEMPLATE_H_
diff --git a/include/scrimmage/plugins/sensor/ROSIMUSensor/IMUErrorSimulator.h b/include/scrimmage/plugins/sensor/ROSIMUSensor/IMUErrorSimulator.h
index 54332954c3..6997aca384 100644
--- a/include/scrimmage/plugins/sensor/ROSIMUSensor/IMUErrorSimulator.h
+++ b/include/scrimmage/plugins/sensor/ROSIMUSensor/IMUErrorSimulator.h
@@ -35,9 +35,8 @@
 
 #include <scrimmage/plugins/sensor/ROSIMUSensor/IMUErrorBudgetTemplate.h>
 
-#include <unsupported/Eigen/MatrixFunctions>
-
 #include <random>
+#include <unsupported/Eigen/MatrixFunctions>
 
 struct NoisyIMUData {
     Eigen::Vector3d noisyDeltaV;
diff --git a/include/scrimmage/plugins/sensor/ROSIMUSensor/ROSIMUSensor.h b/include/scrimmage/plugins/sensor/ROSIMUSensor/ROSIMUSensor.h
index 15bd8a2206..8c59558a11 100644
--- a/include/scrimmage/plugins/sensor/ROSIMUSensor/ROSIMUSensor.h
+++ b/include/scrimmage/plugins/sensor/ROSIMUSensor/ROSIMUSensor.h
@@ -32,24 +32,22 @@
 #ifndef INCLUDE_SCRIMMAGE_PLUGINS_SENSOR_ROSIMUSENSOR_ROSIMUSENSOR_H_
 #define INCLUDE_SCRIMMAGE_PLUGINS_SENSOR_ROSIMUSENSOR_ROSIMUSENSOR_H_
 
+#include <ros/ros.h>
 #include <scrimmage/common/CSV.h>
 #include <scrimmage/math/Quaternion.h>
 #include <scrimmage/parse/MissionParse.h>
 #include <scrimmage/plugins/sensor/ROSIMUSensor/HG4930IMUBudget.h>
 #include <scrimmage/plugins/sensor/ROSIMUSensor/IMUErrorSimulator.h>
 #include <scrimmage/sensor/Sensor.h>
-
-#include <ros/ros.h>
 #include <sensor_msgs/Imu.h>
 
+#include <GeographicLib/Geocentric.hpp>
 #include <map>
 #include <memory>
 #include <random>
 #include <string>
 #include <vector>
 
-#include <GeographicLib/Geocentric.hpp>
-
 namespace scrimmage {
 namespace sensor {
 
@@ -99,17 +97,14 @@ class ROSIMUSensor : public scrimmage::Sensor {
     bool first_sample_collected = false;
     Eigen::Vector3d lla_to_ecef(double lat, double lon, double alt);
     Eigen::Matrix3d enu_to_ecef_rotation(double lat, double lon);
-    Eigen::Vector3d get_deltaV(Eigen::Vector3d pos,
-                               Eigen::Vector3d vel,
-                               Eigen::Quaterniond bodyToEcef,
-                               double deltaT);
+    Eigen::Vector3d get_deltaV(Eigen::Vector3d pos, Eigen::Vector3d vel,
+                               Eigen::Quaterniond bodyToEcef, double deltaT);
     Eigen::Vector3d ecef_to_lla(Eigen::Vector3d ecef);
     Eigen::Matrix3d ecef_to_ned_rotation(double lat, double lon);
     Eigen::Vector3d gravity_ned_from_lla(Eigen::Vector3d lla);
     Eigen::Matrix3d skew_sym(Eigen::Vector3d vector);
     Eigen::Vector3d get_delta_theta(Eigen::Quaterniond qBodyToECEFt1Hat,
-                                    Eigen::Quaterniond qBodyToECEFt2,
-                                    double inertialDeltaT);
+                                    Eigen::Quaterniond qBodyToECEFt2, double inertialDeltaT);
     Eigen::Vector3d inv_skew_sym(Eigen::Matrix3d inputMatrix);
     Eigen::Quaterniond propagate_quaternion(Eigen::Quaterniond qBodyToECEFt1Hat,
                                             Eigen::Vector3d deltaThetaBodyWRTInertialInBody,
diff --git a/include/scrimmage/plugins/sensor/RayTrace/RayTrace.h b/include/scrimmage/plugins/sensor/RayTrace/RayTrace.h
index e60ec874ec..95921e5b9d 100644
--- a/include/scrimmage/plugins/sensor/RayTrace/RayTrace.h
+++ b/include/scrimmage/plugins/sensor/RayTrace/RayTrace.h
@@ -47,18 +47,12 @@ class RayTrace : public scrimmage::Sensor {
  public:
     class PCPoint {
      public:
-        PCPoint()
-            : point(Eigen::Vector3d(0, 0, 0)),
-              intensity(0),
-              oor(false) {}
-        explicit PCPoint(Eigen::Vector3d p)
-            : point(p),
-              intensity(0),
-              oor(false) {}
-        PCPoint(Eigen::Vector3d p, double i, bool o)
-            : point(p),
-              intensity(i),
-              oor(o) {}
+        PCPoint() : point(Eigen::Vector3d(0, 0, 0)), intensity(0), oor(false) {
+        }
+        explicit PCPoint(Eigen::Vector3d p) : point(p), intensity(0), oor(false) {
+        }
+        PCPoint(Eigen::Vector3d p, double i, bool o) : point(p), intensity(i), oor(o) {
+        }
         Eigen::Vector3d point;
         double intensity;
         bool oor;  // Out of range
@@ -69,22 +63,19 @@ class RayTrace : public scrimmage::Sensor {
      */
     class PCRay {
      public:
-        PCRay()
-            : azimuth_rad(0.0),
-              elevation_rad(0.0) {}
+        PCRay() : azimuth_rad(0.0), elevation_rad(0.0) {
+        }
         PCRay(double in_az_rad, double in_el_rad)
-            : azimuth_rad(in_az_rad),
-              elevation_rad(in_el_rad) {}
+            : azimuth_rad(in_az_rad), elevation_rad(in_el_rad) {
+        }
         double azimuth_rad;
         double elevation_rad;
     };
 
     class PointCloud {
      public:
-        PointCloud()
-            : max_range(0),
-              min_range(0),
-              max_sample_rate(0) {}
+        PointCloud() : max_range(0), min_range(0), max_sample_rate(0) {
+        }
 
         std::vector<PCRay> get_rays();
 
@@ -98,10 +89,8 @@ class RayTrace : public scrimmage::Sensor {
     class PointCloudWithId : public PointCloud {
      public:
         PointCloudWithId()
-            : PointCloud(),
-              sensor_name("unknown"),
-              entity_id(0),
-              world_frame(false) {}
+            : PointCloud(), sensor_name("unknown"), entity_id(0), world_frame(false) {
+        }
 
         std::string sensor_name;
         int entity_id;
@@ -111,19 +100,29 @@ class RayTrace : public scrimmage::Sensor {
 
     RayTrace();
 
-    std::string type() override { return "Ray"; }
-    void init(std::map<std::string, std::string> &params) override;
+    std::string type() override {
+        return "Ray";
+    }
+    void init(std::map<std::string, std::string>& params) override;
     bool step() override;
 
     /** Whether to have the bullet collision plugin automatically run ray tracing for this sensor
      * and send associated messages */
-    bool automatic_ray_tracing() { return auto_ray_tracing_; }
+    bool automatic_ray_tracing() {
+        return auto_ray_tracing_;
+    }
     /** Returns a copy of the rays*/
     std::vector<PCRay> rays();
     /** Sensor ranges and update rates */
-    double max_range() { return max_range_; }
-    double min_range() { return min_range_; }
-    double max_sample_rate() { return max_sample_rate_; }
+    double max_range() {
+        return max_range_;
+    }
+    double min_range() {
+        return min_range_;
+    }
+    double max_sample_rate() {
+        return max_sample_rate_;
+    }
 
  protected:
     std::shared_ptr<std::default_random_engine> gener_;
diff --git a/include/scrimmage/plugins/sensor/RigidBody6DOFStateSensor/RigidBody6DOFStateSensor.h b/include/scrimmage/plugins/sensor/RigidBody6DOFStateSensor/RigidBody6DOFStateSensor.h
index 1e558cdd36..7a8fb74656 100644
--- a/include/scrimmage/plugins/sensor/RigidBody6DOFStateSensor/RigidBody6DOFStateSensor.h
+++ b/include/scrimmage/plugins/sensor/RigidBody6DOFStateSensor/RigidBody6DOFStateSensor.h
@@ -35,11 +35,11 @@
 
 #include <scrimmage/sensor/Sensor.h>
 
-#include <random>
-#include <vector>
 #include <map>
-#include <string>
 #include <memory>
+#include <random>
+#include <string>
+#include <vector>
 
 namespace scrimmage {
 namespace motion {
@@ -50,7 +50,7 @@ namespace sensor {
 class RigidBody6DOFStateSensor : public scrimmage::Sensor {
  public:
     RigidBody6DOFStateSensor();
-    void init(std::map<std::string, std::string> &params) override;
+    void init(std::map<std::string, std::string>& params) override;
     bool step() override;
 
  protected:
@@ -62,6 +62,6 @@ class RigidBody6DOFStateSensor : public scrimmage::Sensor {
 
  private:
 };
-} // namespace sensor
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_SENSOR_RIGIDBODY6DOFSTATESENSOR_RIGIDBODY6DOFSTATESENSOR_H_
+}  // namespace sensor
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_SENSOR_RIGIDBODY6DOFSTATESENSOR_RIGIDBODY6DOFSTATESENSOR_H_
diff --git a/include/scrimmage/plugins/sensor/ScrimmageOpenAISensor/ScrimmageOpenAISensor.h b/include/scrimmage/plugins/sensor/ScrimmageOpenAISensor/ScrimmageOpenAISensor.h
index 9f6f5015e1..0de57058aa 100644
--- a/include/scrimmage/plugins/sensor/ScrimmageOpenAISensor/ScrimmageOpenAISensor.h
+++ b/include/scrimmage/plugins/sensor/ScrimmageOpenAISensor/ScrimmageOpenAISensor.h
@@ -33,8 +33,8 @@
 #ifndef INCLUDE_SCRIMMAGE_PLUGINS_SENSOR_SCRIMMAGEOPENAISENSOR_SCRIMMAGEOPENAISENSOR_H_
 #define INCLUDE_SCRIMMAGE_PLUGINS_SENSOR_SCRIMMAGEOPENAISENSOR_SCRIMMAGEOPENAISENSOR_H_
 
-#include <scrimmage/sensor/Sensor.h>
 #include <scrimmage/plugins/autonomy/ScrimmageOpenAIAutonomy/ScrimmageOpenAIAutonomy.h>
+#include <scrimmage/sensor/Sensor.h>
 
 namespace scrimmage {
 namespace sensor {
@@ -42,10 +42,13 @@ class ScrimmageOpenAISensor : public scrimmage::Sensor {
  public:
     EnvParams observation_space;
     ScrimmageOpenAISensor();
-    virtual void set_observation_space() {}
-    virtual void get_observation(int* /*data*/, uint32_t /*beg_idx*/, uint32_t /*end_idx*/) {}
-    virtual void get_observation(double* /*data*/, uint32_t /*beg_idx*/, uint32_t /*end_idx*/) {}
+    virtual void set_observation_space() {
+    }
+    virtual void get_observation(int* /*data*/, uint32_t /*beg_idx*/, uint32_t /*end_idx*/) {
+    }
+    virtual void get_observation(double* /*data*/, uint32_t /*beg_idx*/, uint32_t /*end_idx*/) {
+    }
 };
-} // namespace sensor
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PLUGINS_SENSOR_SCRIMMAGEOPENAISENSOR_SCRIMMAGEOPENAISENSOR_H_
+}  // namespace sensor
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PLUGINS_SENSOR_SCRIMMAGEOPENAISENSOR_SCRIMMAGEOPENAISENSOR_H_
diff --git a/include/scrimmage/plugins/sensor/SimpleINS/SimpleINS.h b/include/scrimmage/plugins/sensor/SimpleINS/SimpleINS.h
index ff44dcbe8a..64d4e46588 100644
--- a/include/scrimmage/plugins/sensor/SimpleINS/SimpleINS.h
+++ b/include/scrimmage/plugins/sensor/SimpleINS/SimpleINS.h
@@ -45,7 +45,7 @@ namespace scrimmage {
 namespace sensor {
 class SimpleINS : public scrimmage::Sensor {
  public:
-    void init(std::map<std::string, std::string> &params) override;
+    void init(std::map<std::string, std::string>& params) override;
     bool step() override;
 
  protected:
diff --git a/include/scrimmage/proto/ProtoConversions.h b/include/scrimmage/proto/ProtoConversions.h
index d19fc465a8..ffc94e0e43 100644
--- a/include/scrimmage/proto/ProtoConversions.h
+++ b/include/scrimmage/proto/ProtoConversions.h
@@ -34,11 +34,10 @@
 #define INCLUDE_SCRIMMAGE_PROTO_PROTOCONVERSIONS_H_
 
 #include <Eigen/Dense>
-
 #include <list>
-#include <vector>
-#include <unordered_map>
 #include <memory>
+#include <unordered_map>
+#include <vector>
 
 namespace scrimmage_proto {
 class Contact;
@@ -53,7 +52,7 @@ class Color;
 class State;
 class Frame;
 class Line;
-} // namespace scrimmage_proto
+}  // namespace scrimmage_proto
 
 namespace scrimmage {
 
@@ -70,58 +69,57 @@ class Contact;
 using ContactMap = std::unordered_map<int, Contact>;
 using ContactMapPtr = std::shared_ptr<ContactMap>;
 
-void set(scrimmage_proto::Vector3d *dst, Eigen::Vector3d src);
-void set(scrimmage_proto::Vector3d *dst, double x, double y, double z);
-void set(scrimmage_proto::Color *dst, const std::vector<int> &src);
-void set(Eigen::Vector3d &dst, const scrimmage_proto::Color &src);
-void set(std::vector<int> &dst, const scrimmage_proto::Color &src);
-void set(scrimmage_proto::Color &dst, const scrimmage_proto::Color &src);
-void set(Eigen::Vector3d &dst, const scrimmage_proto::Vector3d *src);
-void set(scrimmage_proto::Color &dst, scrimmage_proto::Color &src);
-void set(scrimmage_proto::Color &dst, int r, int g, int b);
-void set(scrimmage_proto::Color *dst, const Eigen::Vector3d &color);
-void set(scrimmage_proto::Color *dst, scrimmage_proto::Color *src);
-void set(scrimmage_proto::Color *dst, scrimmage_proto::Color src);
-void set(scrimmage_proto::Color *dst, const scrimmage::Color_t &src);
-void set(scrimmage_proto::Color *color, int r, int g, int b);
-void set(scrimmage_proto::Color *color, int grayscale);
-void set(scrimmage_proto::Quaternion *dst, const Quaternion &src);
-void set(scrimmage_proto::Quaternion *dst, const double &w, const double &x,
-         const double &y, const double &z);
-void set(scrimmage_proto::State *dst, const scrimmage::StatePtr &state);
-void set(scrimmage_proto::State *dst, const scrimmage::State &state);
-void set(scrimmage::State &dst, const scrimmage_proto::State &state);
-void set(scrimmage::Quaternion &dst, const scrimmage_proto::Quaternion &quat);
-
-Eigen::Vector3d eigen(const scrimmage_proto::Vector3d &src);
-
-void add_point_color(std::shared_ptr<scrimmage_proto::Shape> s, const scrimmage::Color_t &c);
-void add_point_color(scrimmage_proto::ShapePtr s, const int &r, const int &g, const int &b);
-void add_point_color(scrimmage_proto::ShapePtr s, const int &grayscale);
-
-void set(scrimmage::ID &id, const scrimmage_proto::ID &proto_id);
-
-Quaternion proto_2_quat(const scrimmage_proto::Quaternion & proto_quat);
-
-void set(Eigen::Vector3d &dst, const scrimmage_proto::Vector3d &proto_vector3d);
-
-Eigen::Vector3d proto_2_vector3d(const scrimmage_proto::Vector3d &proto_vector3d);
-
-StatePtr proto_2_state(const scrimmage_proto::State &proto_state);
-
-void path_to_lines(std::vector<Eigen::Vector3d> &path,
+void set(scrimmage_proto::Vector3d* dst, Eigen::Vector3d src);
+void set(scrimmage_proto::Vector3d* dst, double x, double y, double z);
+void set(scrimmage_proto::Color* dst, const std::vector<int>& src);
+void set(Eigen::Vector3d& dst, const scrimmage_proto::Color& src);
+void set(std::vector<int>& dst, const scrimmage_proto::Color& src);
+void set(scrimmage_proto::Color& dst, const scrimmage_proto::Color& src);
+void set(Eigen::Vector3d& dst, const scrimmage_proto::Vector3d* src);
+void set(scrimmage_proto::Color& dst, scrimmage_proto::Color& src);
+void set(scrimmage_proto::Color& dst, int r, int g, int b);
+void set(scrimmage_proto::Color* dst, const Eigen::Vector3d& color);
+void set(scrimmage_proto::Color* dst, scrimmage_proto::Color* src);
+void set(scrimmage_proto::Color* dst, scrimmage_proto::Color src);
+void set(scrimmage_proto::Color* dst, const scrimmage::Color_t& src);
+void set(scrimmage_proto::Color* color, int r, int g, int b);
+void set(scrimmage_proto::Color* color, int grayscale);
+void set(scrimmage_proto::Quaternion* dst, const Quaternion& src);
+void set(scrimmage_proto::Quaternion* dst, const double& w, const double& x, const double& y,
+         const double& z);
+void set(scrimmage_proto::State* dst, const scrimmage::StatePtr& state);
+void set(scrimmage_proto::State* dst, const scrimmage::State& state);
+void set(scrimmage::State& dst, const scrimmage_proto::State& state);
+void set(scrimmage::Quaternion& dst, const scrimmage_proto::Quaternion& quat);
+
+Eigen::Vector3d eigen(const scrimmage_proto::Vector3d& src);
+
+void add_point_color(std::shared_ptr<scrimmage_proto::Shape> s, const scrimmage::Color_t& c);
+void add_point_color(scrimmage_proto::ShapePtr s, const int& r, const int& g, const int& b);
+void add_point_color(scrimmage_proto::ShapePtr s, const int& grayscale);
+
+void set(scrimmage::ID& id, const scrimmage_proto::ID& proto_id);
+
+Quaternion proto_2_quat(const scrimmage_proto::Quaternion& proto_quat);
+
+void set(Eigen::Vector3d& dst, const scrimmage_proto::Vector3d& proto_vector3d);
+
+Eigen::Vector3d proto_2_vector3d(const scrimmage_proto::Vector3d& proto_vector3d);
+
+StatePtr proto_2_state(const scrimmage_proto::State& proto_state);
+
+void path_to_lines(std::vector<Eigen::Vector3d>& path,
                    std::shared_ptr<scrimmage_proto::Shape> sample_line,
                    std::shared_ptr<EntityPlugin> p);
 
-std::list<scrimmage_proto::Line> points_to_lines(
-    const std::list<Eigen::Vector3d> &points);
+std::list<scrimmage_proto::Line> points_to_lines(const std::list<Eigen::Vector3d>& points);
 
-Contact proto_2_contact(const scrimmage_proto::Contact &proto_contact);
+Contact proto_2_contact(const scrimmage_proto::Contact& proto_contact);
 
-Frame proto_2_frame(const scrimmage_proto::Frame &proto_frame);
+Frame proto_2_frame(const scrimmage_proto::Frame& proto_frame);
 
-std::shared_ptr<scrimmage_proto::Frame>
-create_frame(double time, std::shared_ptr<ContactMap> &contacts);
+std::shared_ptr<scrimmage_proto::Frame> create_frame(double time,
+                                                     std::shared_ptr<ContactMap>& contacts);
 
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PROTO_PROTOCONVERSIONS_H_
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PROTO_PROTOCONVERSIONS_H_
diff --git a/include/scrimmage/pubsub/Message.h b/include/scrimmage/pubsub/Message.h
index c101c02bbc..5e0737c109 100644
--- a/include/scrimmage/pubsub/Message.h
+++ b/include/scrimmage/pubsub/Message.h
@@ -35,21 +35,23 @@
 
 #include <scrimmage/pubsub/MessageBase.h>
 
-#include <string>
 #include <memory>
+#include <string>
 
 namespace scrimmage {
 
 template <class T>
 class Message : public MessageBase {
  public:
-    Message() : MessageBase() {}
-    explicit Message(T _data) : MessageBase(), data(_data) {}
+    Message() : MessageBase() {
+    }
+    explicit Message(T _data) : MessageBase(), data(_data) {
+    }
     T data;
 };
 
-template<class T>
+template <class T>
 using MessagePtr = std::shared_ptr<Message<T>>;
 
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PUBSUB_MESSAGE_H_
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PUBSUB_MESSAGE_H_
diff --git a/include/scrimmage/pubsub/MessageBase.h b/include/scrimmage/pubsub/MessageBase.h
index 010ba5ff55..1e2ef02dae 100644
--- a/include/scrimmage/pubsub/MessageBase.h
+++ b/include/scrimmage/pubsub/MessageBase.h
@@ -33,14 +33,15 @@
 #ifndef INCLUDE_SCRIMMAGE_PUBSUB_MESSAGEBASE_H_
 #define INCLUDE_SCRIMMAGE_PUBSUB_MESSAGEBASE_H_
 
-#include <string>
 #include <memory>
+#include <string>
 
 namespace scrimmage {
 
 class MessageBase {
  public:
-    virtual ~MessageBase() {}       // http://stackoverflow.com/a/5831797
+    virtual ~MessageBase() {
+    }  // http://stackoverflow.com/a/5831797
 
     static const int undefined_id = -1;
     double time;
@@ -51,5 +52,5 @@ class MessageBase {
 
 using MessageBasePtr = std::shared_ptr<MessageBase>;
 
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PUBSUB_MESSAGEBASE_H_
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PUBSUB_MESSAGEBASE_H_
diff --git a/include/scrimmage/pubsub/Network.h b/include/scrimmage/pubsub/Network.h
index 22342e7843..1174c94852 100644
--- a/include/scrimmage/pubsub/Network.h
+++ b/include/scrimmage/pubsub/Network.h
@@ -33,16 +33,16 @@
 #ifndef INCLUDE_SCRIMMAGE_PUBSUB_NETWORK_H_
 #define INCLUDE_SCRIMMAGE_PUBSUB_NETWORK_H_
 
-#include <scrimmage/fwd_decl.h>
-#include <scrimmage/entity/EntityPlugin.h>
 #include <scrimmage/common/CSV.h>
+#include <scrimmage/entity/EntityPlugin.h>
+#include <scrimmage/fwd_decl.h>
 
-#include <map>
 #include <list>
+#include <map>
 #include <memory>
-#include <vector>
 #include <string>
 #include <unordered_map>
+#include <vector>
 
 namespace scrimmage {
 
@@ -50,24 +50,29 @@ class Network : public EntityPlugin {
  public:
     Network();
 
-    virtual bool init(std::map<std::string, std::string> &/*mission_params*/,
-                      std::map<std::string, std::string> &/*plugin_params*/);
-    virtual bool step(std::map<std::string, std::list<NetworkDevicePtr>> &pubs,
-                      std::map<std::string, std::list<NetworkDevicePtr>> &subs);
+    virtual bool init(std::map<std::string, std::string>& /*mission_params*/,
+                      std::map<std::string, std::string>& /*plugin_params*/);
+    virtual bool step(std::map<std::string, std::list<NetworkDevicePtr>>& pubs,
+                      std::map<std::string, std::list<NetworkDevicePtr>>& subs);
     std::string type() override;
 
-    void set_rtree(const RTreePtr &rtree);
+    void set_rtree(const RTreePtr& rtree);
 
     void set_random(RandomPtr random);
 
     void close(double t) override;
 
-    inline virtual void set_mission_parse(MissionParsePtr mp)
-    { mp_ = mp; }
+    inline virtual void set_mission_parse(MissionParsePtr mp) {
+        mp_ = mp;
+    }
 
     // Get/set delay values
-    double comm_delay() {return comm_delay_;}
-    void set_comm_delay(double del) {comm_delay_ = del;}
+    double comm_delay() {
+        return comm_delay_;
+    }
+    void set_comm_delay(double del) {
+        comm_delay_ = del;
+    }
 
  protected:
     RTreePtr rtree_;
@@ -83,16 +88,16 @@ class Network : public EntityPlugin {
     // Value : Whether the publisher can reach the subscriber with a message
     std::unordered_map<int, std::unordered_map<int, bool>> reachable_map_;
 
-    virtual bool is_reachable(const scrimmage::EntityPluginPtr &pub_plugin,
-                              const scrimmage::EntityPluginPtr &sub_plugin);
+    virtual bool is_reachable(const scrimmage::EntityPluginPtr& pub_plugin,
+                              const scrimmage::EntityPluginPtr& sub_plugin);
 
-    virtual bool is_successful_transmission(const scrimmage::EntityPluginPtr &pub_plugin,
-                                            const scrimmage::EntityPluginPtr &sub_plugin);
+    virtual bool is_successful_transmission(const scrimmage::EntityPluginPtr& pub_plugin,
+                                            const scrimmage::EntityPluginPtr& sub_plugin);
 
     virtual double get_transmission_delay();
 
-    bool network_init(std::map<std::string, std::string> &/*mission_params*/,
-                      std::map<std::string, std::string> &/*plugin_params*/);
+    bool network_init(std::map<std::string, std::string>& /*mission_params*/,
+                      std::map<std::string, std::string>& /*plugin_params*/);
 
  private:
     // Key: Topic String
@@ -110,6 +115,6 @@ typedef std::shared_ptr<Network> NetworkPtr;
 using NetworkMap = std::map<std::string, NetworkPtr>;
 using NetworkMapPtr = std::shared_ptr<NetworkMap>;
 
-} // namespace scrimmage
+}  // namespace scrimmage
 
-#endif // INCLUDE_SCRIMMAGE_PUBSUB_NETWORK_H_
+#endif  // INCLUDE_SCRIMMAGE_PUBSUB_NETWORK_H_
diff --git a/include/scrimmage/pubsub/NetworkDevice.h b/include/scrimmage/pubsub/NetworkDevice.h
index 82bf27131d..c83b309e8c 100644
--- a/include/scrimmage/pubsub/NetworkDevice.h
+++ b/include/scrimmage/pubsub/NetworkDevice.h
@@ -33,14 +33,14 @@
 #ifndef INCLUDE_SCRIMMAGE_PUBSUB_NETWORKDEVICE_H_
 #define INCLUDE_SCRIMMAGE_PUBSUB_NETWORKDEVICE_H_
 
-#include <scrimmage/pubsub/MessageBase.h>
 #include <scrimmage/pubsub/Message.h>
+#include <scrimmage/pubsub/MessageBase.h>
 
-#include <type_traits>
 #include <list>
 #include <memory>
+#include <mutex>  // NOLINT
 #include <string>
-#include <mutex> // NOLINT
+#include <type_traits>
 
 namespace scrimmage {
 
@@ -50,18 +50,18 @@ using EntityPluginPtr = std::shared_ptr<EntityPlugin>;
 class NetworkDevice {
  public:
     NetworkDevice();
-    NetworkDevice(NetworkDevice &rhs);
-    NetworkDevice(NetworkDevice &&rhs);
+    NetworkDevice(NetworkDevice& rhs);
+    NetworkDevice(NetworkDevice&& rhs);
 
-    virtual ~NetworkDevice() = default; // Make NetworkDevice Polymorphic
+    virtual ~NetworkDevice() = default;  // Make NetworkDevice Polymorphic
 
-    NetworkDevice(const std::string &topic, const unsigned int& max_queue_size,
+    NetworkDevice(const std::string& topic, const unsigned int& max_queue_size,
                   const bool& enable_queue_size, EntityPluginPtr plugin);
 
     std::string get_topic() const;
-    void set_topic(const std::string &topic);
+    void set_topic(const std::string& topic);
 
-    void set_msg_list(const std::list<MessageBasePtr> &msg_list);
+    void set_msg_list(const std::list<MessageBasePtr>& msg_list);
     void clear_msg_list();
 
     unsigned int msg_list_size() {
@@ -72,7 +72,6 @@ class NetworkDevice {
         return undelivered_msg_list_.size();
     }
 
-
     void set_max_queue_size(const unsigned int& size);
     unsigned int max_queue_size();
     void enable_queue_size(const bool& enforce);
@@ -85,8 +84,7 @@ class NetworkDevice {
     /* added for delay handling */
     void add_undelivered_msg(MessageBasePtr msg, const bool& is_stochastic_delay = false);
     auto deliver_undelivered_msg(std::list<MessageBasePtr>::iterator it);
-    int deliver_undelivered_msg(const double& time_now,
-                                const bool& is_stochastic_delay = false);
+    int deliver_undelivered_msg(const double& time_now, const bool& is_stochastic_delay = false);
     /* end delay handling */
 
     template <class T = MessageBase,
@@ -104,9 +102,9 @@ class NetworkDevice {
         return msg_list_cast;
     }
 
-    template <class T,
-              class = std::enable_if_t<!std::is_same<T, MessageBase>::value &&
-                                       std::is_base_of<MessageBase, T>::value, void>>
+    template <class T, class = std::enable_if_t<!std::is_same<T, MessageBase>::value
+                                                    && std::is_base_of<MessageBase, T>::value,
+                                                void>>
     std::list<std::shared_ptr<T>> pop_msgs() {
         mutex_.lock();
         std::list<std::shared_ptr<T>> msg_list_cast;
@@ -116,9 +114,8 @@ class NetworkDevice {
             auto msg_cast = std::dynamic_pointer_cast<T>(*it);
             if (msg_cast) {
                 msg_list_cast.push_back(msg_cast);
-                } else {
-                print_str(std::string("WARNING: could not cast message on topic \"")
-                          + topic_);
+            } else {
+                print_str(std::string("WARNING: could not cast message on topic \"") + topic_);
             }
             it = msg_list_.erase(it);
         }
@@ -126,59 +123,64 @@ class NetworkDevice {
         return msg_list_cast;
     }
 
-    template <class T,
-              class = std::enable_if_t<!std::is_same<T, MessageBase>::value &&
-                                       !std::is_base_of<MessageBase, T>::value, void>>
+    template <class T, class = std::enable_if_t<!std::is_same<T, MessageBase>::value
+                                                    && !std::is_base_of<MessageBase, T>::value,
+                                                void>>
     std::list<std::shared_ptr<Message<T>>> pop_msgs() {
         return pop_msgs<Message<T>>();
     }
 
     ////////////////////
 
-    template<class T>
-        struct always_false : std::false_type
-    { };
+    template <class T>
+    struct always_false : std::false_type {};
 
     /*! \brief The msgs() method is no longer supported. Please use the new
      *  subscriber callback interface.*/
     template <class T = MessageBase,
               class = std::enable_if_t<std::is_same<T, MessageBase>::value, void>>
     std::list<MessageBasePtr> msgs(bool pop_msgs = true) {
-        static_assert(always_false<T>::value, "You are using the old publisher/subscriber interface (i.e., msgs(bool)) in your plugin. Please use the new subscriber callback interface.");
+        static_assert(always_false<T>::value,
+                      "You are using the old publisher/subscriber interface (i.e., msgs(bool)) in "
+                      "your plugin. Please use the new subscriber callback interface.");
         std::list<std::shared_ptr<T>> msg_list_cast;
         return msg_list_cast;
     }
 
     /*! \brief The msgs() method is no longer supported. Please use the new
      *  subscriber callback interface.*/
-    template <class T,
-              class = std::enable_if_t<!std::is_same<T, MessageBase>::value &&
-                                       std::is_base_of<MessageBase, T>::value, void>>
+    template <class T, class = std::enable_if_t<!std::is_same<T, MessageBase>::value
+                                                    && std::is_base_of<MessageBase, T>::value,
+                                                void>>
     std::list<std::shared_ptr<T>> msgs(bool pop_msgs = true) {
-        static_assert(always_false<T>::value, "You are using the old publisher/subscriber interface (i.e., msgs(bool)) in your plugin. Please use the new subscriber callback interface.");
+        static_assert(always_false<T>::value,
+                      "You are using the old publisher/subscriber interface (i.e., msgs(bool)) in "
+                      "your plugin. Please use the new subscriber callback interface.");
         std::list<std::shared_ptr<T>> msg_list_cast;
         return msg_list_cast;
     }
 
     /*! \brief The msgs() method is no longer supported. Please use the new
      *  subscriber callback interface.*/
-    template <class T,
-              class = std::enable_if_t<!std::is_same<T, MessageBase>::value &&
-                                       !std::is_base_of<MessageBase, T>::value, void>>
+    template <class T, class = std::enable_if_t<!std::is_same<T, MessageBase>::value
+                                                    && !std::is_base_of<MessageBase, T>::value,
+                                                void>>
     std::list<std::shared_ptr<Message<T>>> msgs(bool pop_msgs = true) {
-        static_assert(always_false<T>::value, "You are using the old publisher/subscriber interface (i.e., msgs(bool)) in your plugin. Please use the new subscriber callback interface.");
+        static_assert(always_false<T>::value,
+                      "You are using the old publisher/subscriber interface (i.e., msgs(bool)) in "
+                      "your plugin. Please use the new subscriber callback interface.");
         std::list<std::shared_ptr<T>> msg_list_cast;
         return msg_list_cast;
     }
 
-    EntityPluginPtr & plugin();
+    EntityPluginPtr& plugin();
 
  protected:
     std::string topic_ = "";
     unsigned int max_queue_size_ = 1;
     bool enable_queue_size_ = false;
     EntityPluginPtr plugin_;
-    void print_str(const std::string &msg);
+    void print_str(const std::string& msg);
     std::list<MessageBasePtr> msg_list_;
     std::mutex mutex_;
 
@@ -186,5 +188,5 @@ class NetworkDevice {
     std::list<MessageBasePtr> undelivered_msg_list_;
 };
 using NetworkDevicePtr = std::shared_ptr<NetworkDevice>;
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PUBSUB_NETWORKDEVICE_H_
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PUBSUB_NETWORKDEVICE_H_
diff --git a/include/scrimmage/pubsub/PubSub.h b/include/scrimmage/pubsub/PubSub.h
index bf1b4a4b4a..a861812ee2 100644
--- a/include/scrimmage/pubsub/PubSub.h
+++ b/include/scrimmage/pubsub/PubSub.h
@@ -35,13 +35,14 @@
 
 #include <scrimmage/pubsub/Subscriber.h>
 
-#include <map>
 #include <list>
-#include <string>
+#include <map>
 #include <memory>
+#include <string>
 
 namespace boost {
-template <class T> class optional;
+template <class T>
+class optional;
 }
 
 namespace scrimmage {
@@ -61,49 +62,50 @@ class PubSub {
     // Value : List of NetworkDevicePtr's (publishers or subscribers)
     using TopicMap = std::map<std::string, std::map<std::string, std::list<NetworkDevicePtr>>>;
 
-    TopicMap &pubs() { return pub_map_; }
-    TopicMap &subs() { return sub_map_; }
+    TopicMap& pubs() {
+        return pub_map_;
+    }
+    TopicMap& subs() {
+        return sub_map_;
+    }
 
-    void add_network_name(const std::string &str);
+    void add_network_name(const std::string& str);
 
-    boost::optional<std::list<NetworkDevicePtr>> find_devices(const std::string &network_name,
-                                                              const std::string &topic_name,
-                                                              TopicMap &devs);
+    boost::optional<std::list<NetworkDevicePtr>> find_devices(const std::string& network_name,
+                                                              const std::string& topic_name,
+                                                              TopicMap& devs);
 
-    boost::optional<std::list<NetworkDevicePtr>> find_pubs(const std::string &network_name,
-                                                           const std::string &topic_name);
+    boost::optional<std::list<NetworkDevicePtr>> find_pubs(const std::string& network_name,
+                                                           const std::string& topic_name);
 
-    boost::optional<std::list<NetworkDevicePtr>> find_subs(const std::string &network_name,
-                                                           const std::string &topic_name);
+    boost::optional<std::list<NetworkDevicePtr>> find_subs(const std::string& network_name,
+                                                           const std::string& topic_name);
 
     template <class T, class CallbackFunc>
-    SubscriberBasePtr subscribe(const std::string &network_name,
-                                const std::string &topic,
-                                CallbackFunc callback,
-                                unsigned int max_queue_size,
+    SubscriberBasePtr subscribe(const std::string& network_name, const std::string& topic,
+                                CallbackFunc callback, unsigned int max_queue_size,
                                 bool enable_queue_size, EntityPluginPtr plugin) {
         if (sub_map_.count(network_name) == 0) {
             print_str(std::string("WARNING: Subscriber unable to connect to network (")
-                + network_name + ") on topic (" + topic + ")");
+                      + network_name + ") on topic (" + topic + ")");
         }
 
-        SubscriberBasePtr sub =
-            std::make_shared<Subscriber<T, CallbackFunc>>(
-                topic, max_queue_size, enable_queue_size, plugin, callback);
+        SubscriberBasePtr sub = std::make_shared<Subscriber<T, CallbackFunc>>(
+            topic, max_queue_size, enable_queue_size, plugin, callback);
         sub_map_[network_name][topic].push_back(sub);
         return sub;
     }
 
-    PublisherPtr advertise(const std::string &network_name, const std::string &topic,
-                           const unsigned int& max_queue_size,
-                           const bool& enable_queue_size, EntityPluginPtr plugin);
+    PublisherPtr advertise(const std::string& network_name, const std::string& topic,
+                           const unsigned int& max_queue_size, const bool& enable_queue_size,
+                           EntityPluginPtr plugin);
 
  protected:
     TopicMap pub_map_;
     TopicMap sub_map_;
-    void print_str(const std::string &s);
+    void print_str(const std::string& s);
 };
 using PubSubPtr = std::shared_ptr<PubSub>;
-} // namespace scrimmage
+}  // namespace scrimmage
 
-#endif // INCLUDE_SCRIMMAGE_PUBSUB_PUBSUB_H_
+#endif  // INCLUDE_SCRIMMAGE_PUBSUB_PUBSUB_H_
diff --git a/include/scrimmage/pubsub/Publisher.h b/include/scrimmage/pubsub/Publisher.h
index afd57fa539..2a6e8c48d9 100644
--- a/include/scrimmage/pubsub/Publisher.h
+++ b/include/scrimmage/pubsub/Publisher.h
@@ -35,11 +35,10 @@
 
 #include <scrimmage/pubsub/NetworkDevice.h>
 
+#include <boost/type_index.hpp>
 #include <functional>
-#include <string>
 #include <memory>
-
-#include <boost/type_index.hpp>
+#include <string>
 
 namespace scrimmage {
 
@@ -49,10 +48,11 @@ using MessageBasePtr = std::shared_ptr<MessageBase>;
 class Publisher : public NetworkDevice {
  public:
     Publisher();
-    Publisher(const std::string &topic, const unsigned int &max_queue_size,
+    Publisher(const std::string& topic, const unsigned int& max_queue_size,
               const bool& enable_queue_size, EntityPluginPtr plugin);
 
-    template <class T> void publish(const std::shared_ptr<T> &msg, bool add_debug_info = true) {
+    template <class T>
+    void publish(const std::shared_ptr<T>& msg, bool add_debug_info = true) {
         if (add_debug_info) {
             set_debug_info(msg, boost::typeindex::type_id<T>().pretty_name());
         }
@@ -61,8 +61,8 @@ class Publisher : public NetworkDevice {
     std::function<void(MessageBasePtr)> callback;
 
  protected:
-    void set_debug_info(MessageBasePtr msg, const std::string &type);
+    void set_debug_info(MessageBasePtr msg, const std::string& type);
 };
 
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PUBSUB_PUBLISHER_H_
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PUBSUB_PUBLISHER_H_
diff --git a/include/scrimmage/pubsub/Subscriber.h b/include/scrimmage/pubsub/Subscriber.h
index 6f20676057..04689d2c11 100644
--- a/include/scrimmage/pubsub/Subscriber.h
+++ b/include/scrimmage/pubsub/Subscriber.h
@@ -33,22 +33,19 @@
 #ifndef INCLUDE_SCRIMMAGE_PUBSUB_SUBSCRIBER_H_
 #define INCLUDE_SCRIMMAGE_PUBSUB_SUBSCRIBER_H_
 
-#include <scrimmage/pubsub/SubscriberBase.h>
 #include <scrimmage/pubsub/Message.h>
-
-#include <string>
+#include <scrimmage/pubsub/SubscriberBase.h>
 
 #include <boost/type_index.hpp>
+#include <string>
 
 namespace scrimmage {
 template <class T, class CallbackFunc>
 class Subscriber : public SubscriberBase {
  public:
-    Subscriber(const std::string &topic, unsigned int &max_queue_size,
-               bool enable_queue_size, EntityPluginPtr plugin,
-               CallbackFunc callback)
-        : SubscriberBase(topic, max_queue_size, enable_queue_size, plugin),
-        callback_(callback) {
+    Subscriber(const std::string& topic, unsigned int& max_queue_size, bool enable_queue_size,
+               EntityPluginPtr plugin, CallbackFunc callback)
+        : SubscriberBase(topic, max_queue_size, enable_queue_size, plugin), callback_(callback) {
     }
 
     void accept(scrimmage::MessageBasePtr msg) override {
@@ -63,5 +60,5 @@ class Subscriber : public SubscriberBase {
  protected:
     CallbackFunc callback_;
 };
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PUBSUB_SUBSCRIBER_H_
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PUBSUB_SUBSCRIBER_H_
diff --git a/include/scrimmage/pubsub/SubscriberBase.h b/include/scrimmage/pubsub/SubscriberBase.h
index 8f3ef1ba2c..3916ecbbcf 100644
--- a/include/scrimmage/pubsub/SubscriberBase.h
+++ b/include/scrimmage/pubsub/SubscriberBase.h
@@ -35,22 +35,23 @@
 
 #include <scrimmage/pubsub/NetworkDevice.h>
 
-#include <string>
 #include <memory>
+#include <string>
 
 namespace scrimmage {
 class SubscriberBase : public NetworkDevice {
  public:
     SubscriberBase() = default;
-    SubscriberBase(const std::string &topic, unsigned int &max_queue_size,
-                   bool enable_queue_size, EntityPluginPtr plugin) :
-        NetworkDevice(topic, max_queue_size, enable_queue_size, plugin) {}
+    SubscriberBase(const std::string& topic, unsigned int& max_queue_size, bool enable_queue_size,
+                   EntityPluginPtr plugin)
+        : NetworkDevice(topic, max_queue_size, enable_queue_size, plugin) {
+    }
     virtual void accept(scrimmage::MessageBasePtr msg) = 0;
 
  protected:
-    void print_err(const std::string &type, MessageBasePtr msg) const;
+    void print_err(const std::string& type, MessageBasePtr msg) const;
 };
 
 using SubscriberBasePtr = std::shared_ptr<SubscriberBase>;
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_PUBSUB_SUBSCRIBERBASE_H_
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_PUBSUB_SUBSCRIBERBASE_H_
diff --git a/include/scrimmage/sensor/Sensor.h b/include/scrimmage/sensor/Sensor.h
index f56eede6e2..9ebae382a3 100644
--- a/include/scrimmage/sensor/Sensor.h
+++ b/include/scrimmage/sensor/Sensor.h
@@ -38,18 +38,20 @@
 #include <map>
 #include <memory>
 #include <string>
-#include <vector>
 #include <utility>
+#include <vector>
 
 namespace scrimmage {
 
 class Sensor : public EntityPlugin {
  public:
-    virtual void init(std::map<std::string, std::string> &params);
+    virtual void init(std::map<std::string, std::string>& params);
 
     std::string type() override;
 
-    virtual bool step() {return true;}
+    virtual bool step() {
+        return true;
+    }
 
     virtual scrimmage::MessageBasePtr sensor_msg(double t);
 
@@ -67,8 +69,7 @@ class Sensor : public EntityPlugin {
         auto msg = sensor_msg(t);
         if (msg) {
             auto msg_cast = std::dynamic_pointer_cast<scrimmage::Message<T>>(msg);
-            return msg_cast ?
-                std::shared_ptr<scrimmage::Message<T>>(msg_cast) : nullptr;
+            return msg_cast ? std::shared_ptr<scrimmage::Message<T>>(msg_cast) : nullptr;
         } else {
             return nullptr;
         }
@@ -78,4 +79,4 @@ class Sensor : public EntityPlugin {
 using SensorPtr = std::shared_ptr<Sensor>;
 }  // namespace scrimmage
 
-#endif // INCLUDE_SCRIMMAGE_SENSOR_SENSOR_H_
+#endif  // INCLUDE_SCRIMMAGE_SENSOR_SENSOR_H_
diff --git a/include/scrimmage/simcontrol/EntityInteraction.h b/include/scrimmage/simcontrol/EntityInteraction.h
index 21ab4337e1..40863f5b55 100644
--- a/include/scrimmage/simcontrol/EntityInteraction.h
+++ b/include/scrimmage/simcontrol/EntityInteraction.h
@@ -33,11 +33,10 @@
 #ifndef INCLUDE_SCRIMMAGE_SIMCONTROL_ENTITYINTERACTION_H_
 #define INCLUDE_SCRIMMAGE_SIMCONTROL_ENTITYINTERACTION_H_
 
-#include <Eigen/Dense>
-
-#include <scrimmage/fwd_decl.h>
 #include <scrimmage/entity/EntityPlugin.h>
+#include <scrimmage/fwd_decl.h>
 
+#include <Eigen/Dense>
 #include <list>
 #include <map>
 #include <memory>
@@ -46,23 +45,24 @@
 namespace scrimmage {
 
 class EntityInteraction : public EntityPlugin {
-
  public:
-    inline virtual bool init(std::map<std::string, std::string> &/*mission_params*/,
-                      std::map<std::string, std::string> &/*plugin_params*/)
-    { return true;}
+    inline virtual bool init(std::map<std::string, std::string>& /*mission_params*/,
+                             std::map<std::string, std::string>& /*plugin_params*/) {
+        return true;
+    }
 
-    inline virtual bool step_entity_interaction(std::list<EntityPtr> &/*ents*/,
-                                                double /*t*/, double /*dt*/)
-    { return false; }
+    inline virtual bool step_entity_interaction(std::list<EntityPtr>& /*ents*/, double /*t*/,
+                                                double /*dt*/) {
+        return false;
+    }
 
-    inline virtual bool collision_exists(std::list<EntityPtr> &/*ents*/,
-                                  Eigen::Vector3d &/*p*/)
-    { return false; }
+    inline virtual bool collision_exists(std::list<EntityPtr>& /*ents*/, Eigen::Vector3d& /*p*/) {
+        return false;
+    }
 
  protected:
 };
 
 typedef std::shared_ptr<EntityInteraction> EntityInteractionPtr;
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_SIMCONTROL_ENTITYINTERACTION_H_
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_SIMCONTROL_ENTITYINTERACTION_H_
diff --git a/include/scrimmage/simcontrol/SimControl.h b/include/scrimmage/simcontrol/SimControl.h
index d1ccd14e97..abc893fcca 100644
--- a/include/scrimmage/simcontrol/SimControl.h
+++ b/include/scrimmage/simcontrol/SimControl.h
@@ -33,34 +33,32 @@
 #ifndef INCLUDE_SCRIMMAGE_SIMCONTROL_SIMCONTROL_H_
 #define INCLUDE_SCRIMMAGE_SIMCONTROL_SIMCONTROL_H_
 
-#include <Eigen/Dense>
-
-#include <scrimmage/fwd_decl.h>
-
-#include <scrimmage/common/Timer.h>
 #include <scrimmage/common/DelayedTask.h>
 #include <scrimmage/common/FileSearch.h>
+#include <scrimmage/common/Timer.h>
+#include <scrimmage/fwd_decl.h>
 #include <scrimmage/proto/Shape.pb.h>
 #include <scrimmage/proto/Visual.pb.h>
 
-#include <future> // NOLINT
-#include <memory>
+#include <Eigen/Dense>
+#include <condition_variable>  // NOLINT
 #include <deque>
-#include <vector>
+#include <future>  // NOLINT
+#include <list>
+#include <map>
+#include <memory>
+#include <mutex>  // NOLINT
 #include <set>
 #include <string>
-#include <thread> // NOLINT
-#include <map>
-#include <list>
-#include <mutex> // NOLINT
-#include <condition_variable> // NOLINT
+#include <thread>  // NOLINT
 #include <unordered_map>
+#include <vector>
 
 namespace scrimmage {
 
 typedef std::shared_ptr<scrimmage_proto::ContactVisual> ContactVisualPtr;
 
-enum class EndConditionFlags {TIME = 1, ONE_TEAM = 2, NONE = 3, ALL_DEAD = 4};
+enum class EndConditionFlags { TIME = 1, ONE_TEAM = 2, NONE = 3, ALL_DEAD = 4 };
 
 class SimControl {
  public:
@@ -169,14 +167,14 @@ class SimControl {
     void join();
 
     /*
-    * @brief Provides access to the SimControl's entity plugin.
-    *
-    * After SimControl has been initialized with the init() function, this
-    * function provides access to the entity plugin owned by SimControl. This
-    * entity plugin can be used to access the publish/subscribe bus used during
-    * the simulation. For example, when used during a test, the test can
-    * publish and subscribe to messages used by plugins under test.
-    */
+     * @brief Provides access to the SimControl's entity plugin.
+     *
+     * After SimControl has been initialized with the init() function, this
+     * function provides access to the entity plugin owned by SimControl. This
+     * entity plugin can be used to access the publish/subscribe bus used during
+     * the simulation. For example, when used during a test, the test can
+     * publish and subscribe to messages used by plugins under test.
+     */
     EntityPluginPtr plugin();
 
     /**
@@ -184,7 +182,7 @@ class SimControl {
      *
      * The contact map is indexed by contact ID.
      */
-    void get_contacts(std::unordered_map<int, Contact> &contacts);
+    void get_contacts(std::unordered_map<int, Contact>& contacts);
 
     /**
      * @brief Provides access to the simulated contacts visualization
@@ -192,7 +190,7 @@ class SimControl {
      *
      * The contact visuals map is indexed by contact ID.
      */
-    void get_contact_visuals(std::map<int, ContactVisualPtr> &contact_visuals);
+    void get_contact_visuals(std::map<int, ContactVisualPtr>& contact_visuals);
 
     /**
      * @brief Generate entities based on the current time.
@@ -205,15 +203,15 @@ class SimControl {
     bool generate_entities(const double& t);
 
     /// @brief Generate an entity given the entity description ID.
-    bool generate_entity(const int &ent_desc_id);
+    bool generate_entity(const int& ent_desc_id);
 
     /**
      * @brief Generate an entity given the entity description ID,
      * parameters, and plugin specific params. This is utilized for
      * GenerateEntity publishers.
      */
-    bool generate_entity(const int &ent_desc_id,
-                         std::map<std::string, std::string> &params, AttributeMap &plugin_attr_map);
+    bool generate_entity(const int& ent_desc_id, std::map<std::string, std::string>& params,
+                         AttributeMap& plugin_attr_map);
 
     /// @brief Get the pointer to the MissionParser instance.
     MissionParsePtr mp();
@@ -223,7 +221,7 @@ class SimControl {
      *
      * @param [in] t The simulation time
      */
-    void set_time(const double &t);
+    void set_time(const double& t);
 
     /// @brief Get the current simulation time.
     double t();
@@ -256,23 +254,23 @@ class SimControl {
     bool end_condition_reached();
 
     /// @brief Access the simulation timer instance.
-    Timer &timer();
+    Timer& timer();
 
     /// @brief Access the metrics plugins.
-    std::list<MetricsPtr> & metrics();
+    std::list<MetricsPtr>& metrics();
 
     /// @brief Access the PluginManager instance.
-    PluginManagerPtr &plugin_manager();
+    PluginManagerPtr& plugin_manager();
 
     /// @brief Access the FileSearch instance.
-    FileSearchPtr &file_search();
+    FileSearchPtr& file_search();
 
     struct Task {
         // FIXME: this will be much simpler once there is a
         // step function in Plugin.h
         // In particular, we can get rid of Task::Type and
         // more easily do entity_interaction/network plugins in multiple threads.
-        enum class Type {AUTONOMY, CONTROLLER, MOTION, SENSOR};
+        enum class Type { AUTONOMY, CONTROLLER, MOTION, SENSOR };
 
         Type type;
         double t;
@@ -285,7 +283,7 @@ class SimControl {
      * @brief Set the incoming interface for communication from external
      * visualizers.
      */
-    void set_incoming_interface(InterfacePtr &incoming_interface);
+    void set_incoming_interface(InterfacePtr& incoming_interface);
 
     /// @brief Get the incoming interface.
     InterfacePtr incoming_interface();
@@ -294,13 +292,13 @@ class SimControl {
      * @brief Set the outgoing interface for communication to external
      * visualizers.
      */
-    void set_outgoing_interface(InterfacePtr &outgoing_interface);
+    void set_outgoing_interface(InterfacePtr& outgoing_interface);
 
     /// @brief Get the outgoing interface.
     InterfacePtr outgoing_interface();
 
     /// @brief Access the entities in the simulation.
-    std::list<EntityPtr> &ents();
+    std::list<EntityPtr>& ents();
 
     /// @brief Sends terrain to visualizers and log files.
     void send_terrain();
@@ -388,7 +386,7 @@ class SimControl {
     bool add_tasks(Task::Type type, double t, double dt);
 
     bool run_sensors();
-    bool run_motion(EntityPtr &ent, double t, double dt);
+    bool run_motion(EntityPtr& ent, double t, double dt);
     bool reset_autonomies();
 
     std::shared_ptr<Log> log_;
@@ -400,7 +398,7 @@ class SimControl {
 
     PluginManagerPtr plugin_manager_;
 
-    bool collision_exists(Eigen::Vector3d &p);
+    bool collision_exists(Eigen::Vector3d& p);
 
     std::shared_ptr<GeographicLib::LocalCartesian> proj_;
 
@@ -463,10 +461,9 @@ class SimControl {
     void dec_warp();
 
     bool wait_for_ready();
-    bool check_output(const std::string& output_type,
-                      const std::string& desired_output);
+    bool check_output(const std::string& output_type, const std::string& desired_output);
     bool setup_logging();
-    bool logging_logic(const std::string &s);
+    bool logging_logic(const std::string& s);
     void end_of_simulation();
     void cleanup();
     bool finalize();
@@ -478,5 +475,5 @@ class SimControl {
 
     bool python_enabled_ = false;
 };
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_SIMCONTROL_SIMCONTROL_H_
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_SIMCONTROL_SIMCONTROL_H_
diff --git a/include/scrimmage/simcontrol/SimUtils.h b/include/scrimmage/simcontrol/SimUtils.h
index 319f7b425c..e7522d97f0 100644
--- a/include/scrimmage/simcontrol/SimUtils.h
+++ b/include/scrimmage/simcontrol/SimUtils.h
@@ -35,17 +35,17 @@
 
 #include <scrimmage/fwd_decl.h>
 
-
 #include <functional>
-#include <memory>
-#include <unordered_map>
 #include <list>
-#include <set>
 #include <map>
+#include <memory>
+#include <set>
 #include <string>
+#include <unordered_map>
 
 namespace boost {
-template <class T> class optional;
+template <class T>
+class optional;
 }
 
 namespace scrimmage {
@@ -64,33 +64,30 @@ struct SimUtilsInfo {
     std::shared_ptr<std::unordered_map<int, EntityPtr>> id_to_ent_map;
 };
 
-bool create_ent_inters(const SimUtilsInfo &info,
-                       ContactMapPtr contacts,
-                       std::list<scrimmage_proto::ShapePtr> &shapes,
-                       std::list<EntityInteractionPtr> &ent_inters,
-                       const GlobalServicePtr global_services,
-                       const std::set<std::string> &plugin_tags = {},
-                       std::function<void(std::map<std::string, std::string>&)>
-                       param_override_func = [](std::map<std::string, std::string>&){},
-                       const int& debug_level = 0);
+bool create_ent_inters(
+    const SimUtilsInfo& info, ContactMapPtr contacts, std::list<scrimmage_proto::ShapePtr>& shapes,
+    std::list<EntityInteractionPtr>& ent_inters, const GlobalServicePtr global_services,
+    const std::set<std::string>& plugin_tags = {},
+    std::function<void(std::map<std::string, std::string>&)> param_override_func =
+        [](std::map<std::string, std::string>&) {},
+    const int& debug_level = 0);
 
-bool create_metrics(const SimUtilsInfo &info, ContactMapPtr contacts,
-                    std::list<MetricsPtr> &metrics_list,
-                    const std::set<std::string> &plugin_tags = {},
-                    std::function<void(std::map<std::string, std::string>&)>
-                    param_override_func = [](std::map<std::string, std::string>&){},
-                    const int& debug_level = 0);
+bool create_metrics(
+    const SimUtilsInfo& info, ContactMapPtr contacts, std::list<MetricsPtr>& metrics_list,
+    const std::set<std::string>& plugin_tags = {},
+    std::function<void(std::map<std::string, std::string>&)> param_override_func =
+        [](std::map<std::string, std::string>&) {},
+    const int& debug_level = 0);
 
-bool create_networks(const SimUtilsInfo &info, NetworkMap &networks,
-                     const std::set<std::string> &plugin_tags = {},
-                     std::function<void(std::map<std::string, std::string>&)>
-                     param_override_func = [](std::map<std::string, std::string>&){},
-                     const int& debug_level = 0);
+bool create_networks(
+    const SimUtilsInfo& info, NetworkMap& networks, const std::set<std::string>& plugin_tags = {},
+    std::function<void(std::map<std::string, std::string>&)> param_override_func =
+        [](std::map<std::string, std::string>&) {},
+    const int& debug_level = 0);
 
 void run_callbacks(EntityPluginPtr plugin);
 
-boost::optional<std::string> run_test(const std::string& mission,
-                                      const bool& init_python = true,
+boost::optional<std::string> run_test(const std::string& mission, const bool& init_python = true,
                                       const bool& shutdown_python = true);
 }  // namespace scrimmage
 
diff --git a/include/scrimmage/viewer/CameraInterface.h b/include/scrimmage/viewer/CameraInterface.h
index ae49891f84..0b67845396 100644
--- a/include/scrimmage/viewer/CameraInterface.h
+++ b/include/scrimmage/viewer/CameraInterface.h
@@ -33,22 +33,21 @@
 #ifndef INCLUDE_SCRIMMAGE_VIEWER_CAMERAINTERFACE_H_
 #define INCLUDE_SCRIMMAGE_VIEWER_CAMERAINTERFACE_H_
 
+#include <scrimmage/viewer/Updater.h>
+#include <vtkActor.h>
+#include <vtkCellArray.h>
 #include <vtkCommand.h>
-#include <vtkSmartPointer.h>
-#include <vtkProperty.h>
-#include <vtkVersion.h>
+#include <vtkInteractorStyleTrackballCamera.h>
+#include <vtkObjectFactory.h>
 #include <vtkPoints.h>
-#include <vtkCellArray.h>
 #include <vtkPolyData.h>
 #include <vtkPolyDataMapper.h>
-#include <vtkActor.h>
-#include <vtkRenderWindowInteractor.h>
-#include <vtkInteractorStyleTrackballCamera.h>
-#include <vtkObjectFactory.h>
+#include <vtkProperty.h>
 #include <vtkRenderWindow.h>
+#include <vtkRenderWindowInteractor.h>
 #include <vtkRenderer.h>
-
-#include <scrimmage/viewer/Updater.h>
+#include <vtkSmartPointer.h>
+#include <vtkVersion.h>
 
 #include <string>
 
@@ -70,7 +69,9 @@ class CameraInterface : public vtkInteractorStyleTrackballCamera {
     virtual void Pan();
     virtual void Dolly();
 
-    void set_updater(vtkSmartPointer<Updater> & updater) { updater_ = updater; }
+    void set_updater(vtkSmartPointer<Updater>& updater) {
+        updater_ = updater;
+    }
 
  protected:
     vtkSmartPointer<Updater> updater_;
@@ -78,6 +79,6 @@ class CameraInterface : public vtkInteractorStyleTrackballCamera {
     std::string last_key_;
 };
 // vtkStandardNewMacro(CameraInterface);
-} // namespace scrimmage
+}  // namespace scrimmage
 
-#endif // INCLUDE_SCRIMMAGE_VIEWER_CAMERAINTERFACE_H_
+#endif  // INCLUDE_SCRIMMAGE_VIEWER_CAMERAINTERFACE_H_
diff --git a/include/scrimmage/viewer/Grid.h b/include/scrimmage/viewer/Grid.h
index b59c7b707c..e8d6e48eb2 100644
--- a/include/scrimmage/viewer/Grid.h
+++ b/include/scrimmage/viewer/Grid.h
@@ -33,13 +33,13 @@
 #ifndef INCLUDE_SCRIMMAGE_VIEWER_GRID_H_
 #define INCLUDE_SCRIMMAGE_VIEWER_GRID_H_
 
-#include <vtkSmartPointer.h>
+#include <vtkActor.h>
 #include <vtkLineSource.h>
 #include <vtkPlaneSource.h>
-#include <vtkRenderer.h>
 #include <vtkPolyDataMapper.h>
-#include <vtkActor.h>
 #include <vtkProperty.h>
+#include <vtkRenderer.h>
+#include <vtkSmartPointer.h>
 
 #include <list>
 
@@ -47,8 +47,7 @@ namespace scrimmage {
 
 class Grid {
  public:
-    void create(int size, double spacing,
-                vtkSmartPointer<vtkRenderer> &renderer);
+    void create(int size, double spacing, vtkSmartPointer<vtkRenderer>& renderer);
 
     void remove();
 
@@ -57,6 +56,6 @@ class Grid {
     std::list<vtkSmartPointer<vtkActor> > actors_;
 };
 
-} // namespace scrimmage
+}  // namespace scrimmage
 
-#endif // INCLUDE_SCRIMMAGE_VIEWER_GRID_H_
+#endif  // INCLUDE_SCRIMMAGE_VIEWER_GRID_H_
diff --git a/include/scrimmage/viewer/OriginAxes.h b/include/scrimmage/viewer/OriginAxes.h
index de85cd7361..b4c8113052 100644
--- a/include/scrimmage/viewer/OriginAxes.h
+++ b/include/scrimmage/viewer/OriginAxes.h
@@ -32,21 +32,20 @@
 
 #ifndef INCLUDE_SCRIMMAGE_VIEWER_ORIGINAXES_H_
 #define INCLUDE_SCRIMMAGE_VIEWER_ORIGINAXES_H_
-#include <list>
-
-#include <vtkSmartPointer.h>
-#include <vtkRenderer.h>
 #include <vtkActor.h>
-#include <vtkProperty.h>
 #include <vtkAxesActor.h>
+#include <vtkProperty.h>
+#include <vtkRenderer.h>
+#include <vtkSmartPointer.h>
 #include <vtkTransform.h>
 
+#include <list>
+
 namespace scrimmage {
 
 class OriginAxes {
  public:
-    void create(int length,
-                vtkSmartPointer<vtkRenderer> &renderer);
+    void create(int length, vtkSmartPointer<vtkRenderer>& renderer);
 
     void remove();
 
@@ -55,6 +54,6 @@ class OriginAxes {
     vtkSmartPointer<vtkAxesActor> axes_;
 };
 
-} // namespace scrimmage
+}  // namespace scrimmage
 
-#endif // INCLUDE_SCRIMMAGE_VIEWER_ORIGINAXES_H_
+#endif  // INCLUDE_SCRIMMAGE_VIEWER_ORIGINAXES_H_
diff --git a/include/scrimmage/viewer/Updater.h b/include/scrimmage/viewer/Updater.h
index ba18c9314b..5e87861afb 100644
--- a/include/scrimmage/viewer/Updater.h
+++ b/include/scrimmage/viewer/Updater.h
@@ -40,8 +40,6 @@
 #include <scrimmage/proto/GUIControl.pb.h>
 #include <scrimmage/proto/Shape.pb.h>
 #include <scrimmage/proto/Visual.pb.h>
-
-#include <Eigen/Eigen>
 #include <time.h>
 #include <vtkActor.h>
 #include <vtkCommand.h>
@@ -53,6 +51,7 @@
 #include <vtkSmartPointer.h>
 #include <vtkTextActor.h>
 
+#include <Eigen/Eigen>
 #include <limits>
 #include <list>
 #include <map>
@@ -98,40 +97,42 @@ class Updater : public vtkCommand {
 
     enum class ViewMode { FOLLOW = 0, FREE, OFFSET, FPV };
 
-    static Updater *New() { return new Updater; }
+    static Updater* New() {
+        return new Updater;
+    }
 
     Updater();
 
-    static void set_quat(const scrimmage_proto::Quaternion &quat, vtkSmartPointer<vtkActor> &actor);
-    static void set_quat(const scrimmage::Quaternion &quat, vtkSmartPointer<vtkActor> &actor);
+    static void set_quat(const scrimmage_proto::Quaternion& quat, vtkSmartPointer<vtkActor>& actor);
+    static void set_quat(const scrimmage::Quaternion& quat, vtkSmartPointer<vtkActor>& actor);
 
-    void Execute(vtkObject *caller,
+    void Execute(vtkObject* caller,
                  unsigned long vtkNotUsed(eventId),  // NOLINT
-                 void *vtkNotUsed(callData));
+                 void* vtkNotUsed(callData));
 
     void enable_fps();
 
     bool update();
-    bool update_contacts(std::shared_ptr<scrimmage_proto::Frame> &frame);
-    bool draw_shapes(scrimmage_proto::Shapes &shapes);
+    bool update_contacts(std::shared_ptr<scrimmage_proto::Frame>& frame);
+    bool draw_shapes(scrimmage_proto::Shapes& shapes);
     bool update_scale();
     bool update_camera();
     bool update_text_display();
     bool update_shapes();
 
-    bool update_utm_terrain(std::shared_ptr<scrimmage_proto::UTMTerrain> &utm);
+    bool update_utm_terrain(std::shared_ptr<scrimmage_proto::UTMTerrain>& utm);
 
     void set_max_update_rate(double max_update_rate);
 
-    void set_renderer(vtkSmartPointer<vtkRenderer> &renderer);
+    void set_renderer(vtkSmartPointer<vtkRenderer>& renderer);
 
-    void set_rwi(vtkSmartPointer<vtkRenderWindowInteractor> &rwi);
+    void set_rwi(vtkSmartPointer<vtkRenderWindowInteractor>& rwi);
 
-    void set_incoming_interface(InterfacePtr &incoming_interface);
+    void set_incoming_interface(InterfacePtr& incoming_interface);
 
-    void set_outgoing_interface(InterfacePtr &outgoing_interface);
+    void set_outgoing_interface(InterfacePtr& outgoing_interface);
 
-    void init(const std::string &log_dir, double dt);
+    void init(const std::string& log_dir, double dt);
 
     void next_mode();
 
@@ -139,7 +140,7 @@ class Updater : public vtkCommand {
 
     void track_camera_pos();
 
-    void process_custom_key(std::string &key);
+    void process_custom_key(std::string& key);
 
     void inc_follow();
     void dec_follow();
@@ -168,14 +169,16 @@ class Updater : public vtkCommand {
     void inc_follow_offset();
     void dec_follow_offset();
 
-    void world_point_clicked(const double &x, const double &y, const double &z);
+    void world_point_clicked(const double& x, const double& y, const double& z);
 
     void reset_scale();
-    void set_init_scale(double init_scale) { init_scale_ = init_scale; }
+    void set_init_scale(double init_scale) {
+        init_scale_ = init_scale;
+    }
 
     void set_reset_camera();
-    void set_camera_reset_params(
-        double pos_x, double pos_y, double pos_z, double focal_x, double focal_y, double focal_z);
+    void set_camera_reset_params(double pos_x, double pos_y, double pos_z, double focal_x,
+                                 double focal_y, double focal_z);
 
     void set_view_mode(ViewMode view_mode);
     void set_show_fps(bool show_fps);
@@ -185,110 +188,79 @@ class Updater : public vtkCommand {
 
     void create_text_display();
 
-    void update_trail(std::shared_ptr<ActorContact> &actor_contact,
-                      double &x_pos,
-                      double &y_pos,
-                      double &z_pos);
-
-    void update_contact_visual(std::shared_ptr<ActorContact> &actor_contact,
-                               std::shared_ptr<scrimmage_proto::ContactVisual> &cv);
-
-    void quat_2_transform(const Quaternion &quat, vtkSmartPointer<vtkTransform> transform);
-
-    bool draw_arc(const bool &new_shape,
-                  const scrimmage_proto::Arc &a,
-                  vtkSmartPointer<vtkActor> &actor,
-                  vtkSmartPointer<vtkPolyDataAlgorithm> &source,
-                  vtkSmartPointer<vtkPolyDataMapper> &mapper);
-    bool draw_arrow(const bool &new_shape,
-                    const scrimmage_proto::Arrow &a,
-                    vtkSmartPointer<vtkActor> &actor,
-                    vtkSmartPointer<vtkPolyDataAlgorithm> &source,
-                    vtkSmartPointer<vtkPolyDataMapper> &mapper);
-    bool draw_circle(const bool &new_shape,
-                     const scrimmage_proto::Circle &c,
-                     vtkSmartPointer<vtkActor> &actor,
-                     vtkSmartPointer<vtkPolyDataAlgorithm> &source,
-                     vtkSmartPointer<vtkPolyDataMapper> &mapper);
-    bool draw_cone(const bool &new_shape,
-                   const scrimmage_proto::Cone &c,
-                   vtkSmartPointer<vtkActor> &actor,
-                   vtkSmartPointer<vtkPolyDataAlgorithm> &source,
-                   vtkSmartPointer<vtkPolyDataMapper> &mapper);
-    bool draw_cube(const bool &new_shape,
-                   const scrimmage_proto::Cuboid &c,
-                   vtkSmartPointer<vtkActor> &actor,
-                   vtkSmartPointer<vtkPolyDataAlgorithm> &source,
-                   vtkSmartPointer<vtkPolyDataMapper> &mapper);
-    bool draw_ellipse(const bool &new_shape,
-                      const scrimmage_proto::Ellipse &elp,
-                      vtkSmartPointer<vtkActor> &actor,
-                      vtkSmartPointer<vtkPolyDataAlgorithm> &source,
-                      vtkSmartPointer<vtkPolyDataMapper> &mapper);
-    bool draw_line(const bool &new_shape,
-                   const scrimmage_proto::Line &l,
-                   vtkSmartPointer<vtkActor> &actor,
-                   vtkSmartPointer<vtkPolyDataAlgorithm> &source,
-                   vtkSmartPointer<vtkPolyDataMapper> &mapper);
-    bool draw_mesh(const bool &new_shape,
-                   const scrimmage_proto::Mesh &m,
-                   vtkSmartPointer<vtkActor> &actor,
-                   vtkSmartPointer<vtkPolyDataAlgorithm> &source,
-                   vtkSmartPointer<vtkPolyDataMapper> &mapper);
-    bool draw_plane(const bool &new_shape,
-                    const scrimmage_proto::Plane &p,
-                    vtkSmartPointer<vtkActor> &actor,
-                    vtkSmartPointer<vtkPolyDataAlgorithm> &source,
-                    vtkSmartPointer<vtkPolyDataMapper> &mapper);
-    bool draw_pointcloud(const bool &new_shape,
-                         const scrimmage_proto::Shape &shape,
-                         vtkSmartPointer<vtkActor> &actor,
-                         vtkSmartPointer<vtkPolyDataAlgorithm> &source,
-                         vtkSmartPointer<vtkPolyDataMapper> &mapper);
-    bool draw_polydata(const bool &new_shape,
-                       const scrimmage_proto::Polydata &p,
-                       vtkSmartPointer<vtkActor> &actor,
-                       vtkSmartPointer<vtkPolyDataAlgorithm> &source,
-                       vtkSmartPointer<vtkPolyDataMapper> &mapper);
-    bool draw_polygon(const bool &new_shape,
-                      const scrimmage_proto::Polygon &p,
-                      vtkSmartPointer<vtkActor> &actor,
-                      vtkSmartPointer<vtkPolyDataAlgorithm> &source,
-                      vtkSmartPointer<vtkPolyDataMapper> &mapper);
-    bool draw_polyline(const bool &new_shape,
-                       const scrimmage_proto::Polyline &pl,
-                       vtkSmartPointer<vtkActor> &actor,
-                       vtkSmartPointer<vtkPolyDataAlgorithm> &source,
-                       vtkSmartPointer<vtkPolyDataMapper> &mapper);
-    bool draw_sphere(const bool &new_shape,
-                     const scrimmage_proto::Sphere &s,
-                     vtkSmartPointer<vtkActor> &actor,
-                     vtkSmartPointer<vtkPolyDataAlgorithm> &source,
-                     vtkSmartPointer<vtkPolyDataMapper> &mapper);
-    bool draw_spline(const bool &new_shape,
-                     const scrimmage_proto::Spline &s,
-                     vtkSmartPointer<vtkActor> &actor,
-                     vtkSmartPointer<vtkPolyDataAlgorithm> &source,
-                     vtkSmartPointer<vtkPolyDataMapper> &mapper);
-    bool draw_text(const bool &new_shape,
-                   const scrimmage_proto::Text &t,
-                   vtkSmartPointer<vtkActor> &actor,
-                   vtkSmartPointer<vtkPolyDataAlgorithm> &source,
-                   vtkSmartPointer<vtkPolyDataMapper> &mapper);
-    bool draw_triangle(const bool &new_shape,
-                       const scrimmage_proto::Triangle &t,
-                       vtkSmartPointer<vtkActor> &actor,
-                       vtkSmartPointer<vtkPolyDataAlgorithm> &source,
-                       vtkSmartPointer<vtkPolyDataMapper> &mapper);
+    void update_trail(std::shared_ptr<ActorContact>& actor_contact, double& x_pos, double& y_pos,
+                      double& z_pos);
+
+    void update_contact_visual(std::shared_ptr<ActorContact>& actor_contact,
+                               std::shared_ptr<scrimmage_proto::ContactVisual>& cv);
+
+    void quat_2_transform(const Quaternion& quat, vtkSmartPointer<vtkTransform> transform);
+
+    bool draw_arc(const bool& new_shape, const scrimmage_proto::Arc& a,
+                  vtkSmartPointer<vtkActor>& actor, vtkSmartPointer<vtkPolyDataAlgorithm>& source,
+                  vtkSmartPointer<vtkPolyDataMapper>& mapper);
+    bool draw_arrow(const bool& new_shape, const scrimmage_proto::Arrow& a,
+                    vtkSmartPointer<vtkActor>& actor, vtkSmartPointer<vtkPolyDataAlgorithm>& source,
+                    vtkSmartPointer<vtkPolyDataMapper>& mapper);
+    bool draw_circle(const bool& new_shape, const scrimmage_proto::Circle& c,
+                     vtkSmartPointer<vtkActor>& actor,
+                     vtkSmartPointer<vtkPolyDataAlgorithm>& source,
+                     vtkSmartPointer<vtkPolyDataMapper>& mapper);
+    bool draw_cone(const bool& new_shape, const scrimmage_proto::Cone& c,
+                   vtkSmartPointer<vtkActor>& actor, vtkSmartPointer<vtkPolyDataAlgorithm>& source,
+                   vtkSmartPointer<vtkPolyDataMapper>& mapper);
+    bool draw_cube(const bool& new_shape, const scrimmage_proto::Cuboid& c,
+                   vtkSmartPointer<vtkActor>& actor, vtkSmartPointer<vtkPolyDataAlgorithm>& source,
+                   vtkSmartPointer<vtkPolyDataMapper>& mapper);
+    bool draw_ellipse(const bool& new_shape, const scrimmage_proto::Ellipse& elp,
+                      vtkSmartPointer<vtkActor>& actor,
+                      vtkSmartPointer<vtkPolyDataAlgorithm>& source,
+                      vtkSmartPointer<vtkPolyDataMapper>& mapper);
+    bool draw_line(const bool& new_shape, const scrimmage_proto::Line& l,
+                   vtkSmartPointer<vtkActor>& actor, vtkSmartPointer<vtkPolyDataAlgorithm>& source,
+                   vtkSmartPointer<vtkPolyDataMapper>& mapper);
+    bool draw_mesh(const bool& new_shape, const scrimmage_proto::Mesh& m,
+                   vtkSmartPointer<vtkActor>& actor, vtkSmartPointer<vtkPolyDataAlgorithm>& source,
+                   vtkSmartPointer<vtkPolyDataMapper>& mapper);
+    bool draw_plane(const bool& new_shape, const scrimmage_proto::Plane& p,
+                    vtkSmartPointer<vtkActor>& actor, vtkSmartPointer<vtkPolyDataAlgorithm>& source,
+                    vtkSmartPointer<vtkPolyDataMapper>& mapper);
+    bool draw_pointcloud(const bool& new_shape, const scrimmage_proto::Shape& shape,
+                         vtkSmartPointer<vtkActor>& actor,
+                         vtkSmartPointer<vtkPolyDataAlgorithm>& source,
+                         vtkSmartPointer<vtkPolyDataMapper>& mapper);
+    bool draw_polydata(const bool& new_shape, const scrimmage_proto::Polydata& p,
+                       vtkSmartPointer<vtkActor>& actor,
+                       vtkSmartPointer<vtkPolyDataAlgorithm>& source,
+                       vtkSmartPointer<vtkPolyDataMapper>& mapper);
+    bool draw_polygon(const bool& new_shape, const scrimmage_proto::Polygon& p,
+                      vtkSmartPointer<vtkActor>& actor,
+                      vtkSmartPointer<vtkPolyDataAlgorithm>& source,
+                      vtkSmartPointer<vtkPolyDataMapper>& mapper);
+    bool draw_polyline(const bool& new_shape, const scrimmage_proto::Polyline& pl,
+                       vtkSmartPointer<vtkActor>& actor,
+                       vtkSmartPointer<vtkPolyDataAlgorithm>& source,
+                       vtkSmartPointer<vtkPolyDataMapper>& mapper);
+    bool draw_sphere(const bool& new_shape, const scrimmage_proto::Sphere& s,
+                     vtkSmartPointer<vtkActor>& actor,
+                     vtkSmartPointer<vtkPolyDataAlgorithm>& source,
+                     vtkSmartPointer<vtkPolyDataMapper>& mapper);
+    bool draw_spline(const bool& new_shape, const scrimmage_proto::Spline& s,
+                     vtkSmartPointer<vtkActor>& actor,
+                     vtkSmartPointer<vtkPolyDataAlgorithm>& source,
+                     vtkSmartPointer<vtkPolyDataMapper>& mapper);
+    bool draw_text(const bool& new_shape, const scrimmage_proto::Text& t,
+                   vtkSmartPointer<vtkActor>& actor, vtkSmartPointer<vtkPolyDataAlgorithm>& source,
+                   vtkSmartPointer<vtkPolyDataMapper>& mapper);
+    bool draw_triangle(const bool& new_shape, const scrimmage_proto::Triangle& t,
+                       vtkSmartPointer<vtkActor>& actor,
+                       vtkSmartPointer<vtkPolyDataAlgorithm>& source,
+                       vtkSmartPointer<vtkPolyDataMapper>& mapper);
 
  protected:
-    void get_model_texture(std::string name,
-                           std::string &model_file,
-                           bool &model_found,
-                           std::string &texture_file,
-                           bool &texture_found,
-                           double &base_scale,
-                           Quaternion &base_rot);
+    void get_model_texture(std::string name, std::string& model_file, bool& model_found,
+                           std::string& texture_file, bool& texture_found, double& base_scale,
+                           Quaternion& base_rot);
 
  protected:
     vtkSmartPointer<vtkRenderWindowInteractor> rwi_;
@@ -333,11 +305,8 @@ class Updater : public vtkCommand {
 
     std::map<int, std::shared_ptr<scrimmage_proto::ContactVisual>> contact_visuals_;
 
-    std::unordered_map<uint64_t,
-                       std::tuple<scrimmage_proto::Shape,
-                                  vtkSmartPointer<vtkActor>,
-                                  vtkSmartPointer<vtkPolyDataAlgorithm>,
-                                  double>>
+    std::unordered_map<uint64_t, std::tuple<scrimmage_proto::Shape, vtkSmartPointer<vtkActor>,
+                                            vtkSmartPointer<vtkPolyDataAlgorithm>, double>>
         shapes_;
 
     std::map<std::string, std::shared_ptr<scrimmage_proto::UTMTerrain>> terrain_map_;
diff --git a/include/scrimmage/viewer/Viewer.h b/include/scrimmage/viewer/Viewer.h
index f867397766..5af23a991d 100644
--- a/include/scrimmage/viewer/Viewer.h
+++ b/include/scrimmage/viewer/Viewer.h
@@ -33,17 +33,16 @@
 #ifndef INCLUDE_SCRIMMAGE_VIEWER_VIEWER_H_
 #define INCLUDE_SCRIMMAGE_VIEWER_VIEWER_H_
 
-#include <vtkSmartPointer.h>
+#include <scrimmage/viewer/CameraInterface.h>
 #include <vtkRenderWindow.h>
-#include <vtkRenderer.h>
 #include <vtkRenderWindowInteractor.h>
-
-#include <scrimmage/viewer/CameraInterface.h>
+#include <vtkRenderer.h>
+#include <vtkSmartPointer.h>
 
 #include <map>
-#include <string>
-#include <thread> // NOLINT
 #include <memory>
+#include <string>
+#include <thread>  // NOLINT
 
 namespace scrimmage {
 
@@ -55,9 +54,9 @@ class Viewer {
  public:
     Viewer();
 
-    void set_incoming_interface(InterfacePtr &incoming_interface);
+    void set_incoming_interface(InterfacePtr& incoming_interface);
 
-    void set_outgoing_interface(InterfacePtr &outgoing_interface);
+    void set_outgoing_interface(InterfacePtr& outgoing_interface);
 
     void set_enable_network(bool enable);
 
@@ -91,5 +90,5 @@ class Viewer {
     double init_scale_ = 1.0;
 };
 
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_VIEWER_VIEWER_H_
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_VIEWER_VIEWER_H_
diff --git a/src/autonomy/Autonomy.cpp b/src/autonomy/Autonomy.cpp
index 6af0b9375d..9d1d36405c 100644
--- a/src/autonomy/Autonomy.cpp
+++ b/src/autonomy/Autonomy.cpp
@@ -34,57 +34,92 @@
 #include <scrimmage/entity/Contact.h>
 #include <scrimmage/math/State.h>
 
+#include <GeographicLib/LocalCartesian.hpp>
 #include <memory>
 #include <unordered_map>
 
-#include <GeographicLib/LocalCartesian.hpp>
-
 namespace scrimmage {
 
-Autonomy::Autonomy() : state_(std::make_shared<State>()),
-    desired_state_(std::make_shared<State>()), need_reset_(false), is_controlling_(false) {}
+Autonomy::Autonomy()
+    : state_(std::make_shared<State>()),
+      desired_state_(std::make_shared<State>()),
+      need_reset_(false),
+      is_controlling_(false) {
+}
 
-void Autonomy::set_contacts(ContactMapPtr &contacts) {
+void Autonomy::set_contacts(ContactMapPtr& contacts) {
     contacts_ = contacts;
 }
 
-void Autonomy::set_contacts_from_plugin(AutonomyPtr &ptr) {
+void Autonomy::set_contacts_from_plugin(AutonomyPtr& ptr) {
     contacts_ = ptr->contacts_;
 }
 
-RTreePtr &Autonomy::rtree() {return rtree_;}
+RTreePtr& Autonomy::rtree() {
+    return rtree_;
+}
 
-void Autonomy::set_rtree(RTreePtr &rtree) {rtree_ = rtree;}
+void Autonomy::set_rtree(RTreePtr& rtree) {
+    rtree_ = rtree;
+}
 
-StatePtr &Autonomy::state() {return state_;}
+StatePtr& Autonomy::state() {
+    return state_;
+}
 
-void Autonomy::set_state(StatePtr &state) {state_ = state;}
+void Autonomy::set_state(StatePtr& state) {
+    state_ = state;
+}
 
-void Autonomy::set_projection(std::shared_ptr<GeographicLib::LocalCartesian> &proj) {
+void Autonomy::set_projection(std::shared_ptr<GeographicLib::LocalCartesian>& proj) {
     proj_ = proj;
 }
 
-std::string &Autonomy::logging_msg() {return logging_msg_;}
+std::string& Autonomy::logging_msg() {
+    return logging_msg_;
+}
 
-bool Autonomy::get_is_controlling() {return is_controlling_;}
+bool Autonomy::get_is_controlling() {
+    return is_controlling_;
+}
 
-void Autonomy::set_is_controlling(bool is_controlling) {is_controlling_ = is_controlling;}
+void Autonomy::set_is_controlling(bool is_controlling) {
+    is_controlling_ = is_controlling;
+}
 
-std::string Autonomy::type() { return std::string("Autonomy"); }
+std::string Autonomy::type() {
+    return std::string("Autonomy");
+}
 
-bool Autonomy::step_autonomy(double /*t*/, double /*dt*/) { return true; }
-bool Autonomy::posthumous(double /*t*/) { return true; }
-void Autonomy::init() {}
-void Autonomy::init(std::map<std::string, std::string> &/*params*/) {}
-bool Autonomy::need_reset() {return need_reset_;}
+bool Autonomy::step_autonomy(double /*t*/, double /*dt*/) {
+    return true;
+}
+bool Autonomy::posthumous(double /*t*/) {
+    return true;
+}
+void Autonomy::init() {
+}
+void Autonomy::init(std::map<std::string, std::string>& /*params*/) {
+}
+bool Autonomy::need_reset() {
+    return need_reset_;
+}
 
-StatePtr &Autonomy::desired_state() {return desired_state_;}
+StatePtr& Autonomy::desired_state() {
+    return desired_state_;
+}
 
-void Autonomy::set_desired_state(StatePtr desired_state) {desired_state_ = desired_state;}
+void Autonomy::set_desired_state(StatePtr desired_state) {
+    desired_state_ = desired_state;
+}
 
-ContactMapPtr &Autonomy::get_contacts() {return contacts_;}
+ContactMapPtr& Autonomy::get_contacts() {
+    return contacts_;
+}
 
-ContactMap &Autonomy::get_contacts_raw() {return *contacts_;}
+ContactMap& Autonomy::get_contacts_raw() {
+    return *contacts_;
+}
 
 void Autonomy::close(double /*t*/) {
     proj_ = nullptr;
@@ -93,4 +128,4 @@ void Autonomy::close(double /*t*/) {
     contacts_ = nullptr;
     rtree_ = nullptr;
 }
-} // namespace scrimmage
+}  // namespace scrimmage
diff --git a/src/common/Battery.cpp b/src/common/Battery.cpp
index 9a6e83b632..4e994b9fe2 100644
--- a/src/common/Battery.cpp
+++ b/src/common/Battery.cpp
@@ -31,11 +31,11 @@
  */
 #include <scrimmage/common/Battery.h>
 
-#include <limits>
 #include <cmath>
+#include <limits>
 
 namespace scrimmage {
-Battery::Battery(const double &min, const double &max, const double &current)
+Battery::Battery(const double& min, const double& max, const double& current)
     : min_charge_(min), max_charge_(max), current_charge_(current) {
     if (current_charge_ > max_charge_) {
         current_charge_ = max_charge_;
@@ -44,14 +44,14 @@ Battery::Battery(const double &min, const double &max, const double &current)
         current_charge_ = min_charge_;
     }
 }
-void Battery::add_charge(const double &amount) {
+void Battery::add_charge(const double& amount) {
     current_charge_ += amount;
     if (current_charge_ > max_charge_) {
         current_charge_ = max_charge_;
     }
 }
 
-bool Battery::deplete(const double &amount) {
+bool Battery::deplete(const double& amount) {
     current_charge_ -= amount;
 
     if (current_charge_ < min_charge_) {
@@ -62,19 +62,18 @@ bool Battery::deplete(const double &amount) {
 }
 
 bool Battery::is_full() {
-    return std::abs(current_charge_ - max_charge_) <
-        std::numeric_limits<double>::epsilon();
+    return std::abs(current_charge_ - max_charge_) < std::numeric_limits<double>::epsilon();
 }
 
 bool Battery::has_charge() {
     return current_charge_ > min_charge_;
 }
 
-const double & Battery::current_charge() {
+const double& Battery::current_charge() {
     return current_charge_;
 }
 
 double Battery::charge_percentage() {
     return 100.0 * (current_charge_ - min_charge_) / (max_charge_ - min_charge_);
 }
-} // namespace scrimmage
+}  // namespace scrimmage
diff --git a/src/common/CSV.cpp b/src/common/CSV.cpp
index d3aef72345..170efa3de1 100644
--- a/src/common/CSV.cpp
+++ b/src/common/CSV.cpp
@@ -33,14 +33,13 @@
 #include <scrimmage/common/CSV.h>
 #include <scrimmage/parse/ParseUtils.h>
 
+#include <boost/algorithm/string.hpp>
+#include <boost/tokenizer.hpp>
+#include <fstream>
 #include <iomanip>
 #include <iostream>
-#include <vector>
-#include <fstream>
 #include <sstream>
-
-#include <boost/tokenizer.hpp>
-#include <boost/algorithm/string.hpp>
+#include <vector>
 
 using std::cout;
 using std::endl;
@@ -51,11 +50,11 @@ CSV::~CSV() {
     this->close_output();
 }
 
-void CSV::set_column_headers(const Headers &headers, bool write) {
+void CSV::set_column_headers(const Headers& headers, bool write) {
     column_headers_.clear();
 
     int i = 0;
-    for (const std::string &header : headers) {
+    for (const std::string& header : headers) {
         column_headers_[header] = i;
         i++;
     }
@@ -69,18 +68,16 @@ void CSV::set_column_headers(const Headers &headers, bool write) {
     }
 }
 
-void CSV::set_column_headers(const std::string &headers, bool write) {
+void CSV::set_column_headers(const std::string& headers, bool write) {
     std::list<std::string> headers_vec = get_csv_line_elements(headers);
     set_column_headers(headers_vec, write);
 }
 
-bool CSV::append(const Pairs &pairs, bool write, bool keep_in_memory) {
-
+bool CSV::append(const Pairs& pairs, bool write, bool keep_in_memory) {
     for (std::pair<std::string, double> pair : pairs) {
         auto it = column_headers_.find(pair.first);
         if (it == column_headers_.end()) {
-            cout << "Warning: column header doesn't exist: "
-                 << pair.first << endl;
+            cout << "Warning: column header doesn't exist: " << pair.first << endl;
         }
         table_[next_row_][it->second] = pair.second;
     }
@@ -98,7 +95,7 @@ bool CSV::append(const Pairs &pairs, bool write, bool keep_in_memory) {
     return true;
 }
 
-bool CSV::open_output(const std::string &filename, std::ios_base::openmode mode) {
+bool CSV::open_output(const std::string& filename, std::ios_base::openmode mode) {
     file_out_.open(filename, mode);
     return file_out_.is_open();
 }
@@ -128,7 +125,7 @@ std::string CSV::headers_to_string() const {
 
     // Get an ordered vector of headers
     std::vector<std::string> headers(column_headers_.size());
-    for (auto &kv : column_headers_) {
+    for (auto& kv : column_headers_) {
         headers[kv.second] = kv.first;
     }
 
@@ -136,7 +133,7 @@ std::string CSV::headers_to_string() const {
     for (std::string header : headers) {
         result += header;
 
-        if (i+1 < headers.size()) {
+        if (i + 1 < headers.size()) {
             result += ",";
         }
         i++;
@@ -163,14 +160,18 @@ std::string CSV::row_to_string(const int& row) const {
     // column_headers, use column index to fill in column for values.
     std::vector<std::string> values(column_headers_.size(), no_value_str_);
     auto it_row = table_.find(row);
-    for (auto &kv : it_row->second) {
+    for (auto& kv : it_row->second) {
         if (static_cast<int64_t>(kv.second) == kv.second) {
             values[kv.first] = std::to_string(static_cast<int64_t>(kv.second));
         } else if (static_cast<double>(kv.second) == kv.second) {
             // default precision values for double are not enough in many cases
             std::ostringstream conv;
-            if (double_is_fixed_) { conv << std::fixed; }
-            if (double_is_scientific_) { conv << std::scientific; }
+            if (double_is_fixed_) {
+                conv << std::fixed;
+            }
+            if (double_is_scientific_) {
+                conv << std::scientific;
+            }
             conv << std::setprecision(double_precision_) << kv.second;
             values[kv.first] = conv.str();
         } else {
@@ -183,7 +184,7 @@ std::string CSV::row_to_string(const int& row) const {
     for (std::string str : values) {
         result += str;
 
-        if (i+1 < values.size()) {
+        if (i + 1 < values.size()) {
             result += ",";
         }
         i++;
@@ -191,8 +192,7 @@ std::string CSV::row_to_string(const int& row) const {
     return result;
 }
 
-
-bool CSV::to_csv(const std::string &filename) {
+bool CSV::to_csv(const std::string& filename) {
     if (not this->open_output(filename)) {
         cout << "CSV::to_csv: Failed to open file: " << filename << endl;
         return false;
@@ -211,7 +211,7 @@ bool CSV::to_csv(const std::string &filename) {
     return true;
 }
 
-bool CSV::read_csv_from_string(const std::string &csv_str, const bool& contains_header) {
+bool CSV::read_csv_from_string(const std::string& csv_str, const bool& contains_header) {
     table_.clear();
     column_headers_.clear();
 
@@ -222,12 +222,12 @@ bool CSV::read_csv_from_string(const std::string &csv_str, const bool& contains_
         // Strip "whitespace" characters.
         std::string line = remove_whitespace(line_tokens[line_num]);
 
-        if (line == "") continue; // Ignore lines that are empty
+        if (line == "") continue;  // Ignore lines that are empty
 
         if (line_num == 0 && contains_header) {
             std::list<std::string> headers = get_csv_line_elements(line);
             int i = 0;
-            for (auto &str : headers) {
+            for (auto& str : headers) {
                 column_headers_[str] = i;
                 i++;
             }
@@ -250,10 +250,10 @@ bool CSV::read_csv_from_string(const std::string &csv_str, const bool& contains_
             // Print a warning if the number of comma separated values doesn't
             // match the number of columns
             if (column_headers_.size() != tokens.size()) {
-                cout << "Warning the number of values (" << tokens.size()
-                     << ") on line number " << row_num
-                     << " doesn't match the number of column headers: "
-                     << column_headers_.size() << endl;
+                cout << "Warning the number of values (" << tokens.size() << ") on line number "
+                     << row_num
+                     << " doesn't match the number of column headers: " << column_headers_.size()
+                     << endl;
             }
             row_num++;
         }
@@ -261,7 +261,7 @@ bool CSV::read_csv_from_string(const std::string &csv_str, const bool& contains_
     return true;
 }
 
-bool CSV::read_csv(const std::string &filename, const bool& contains_header) {
+bool CSV::read_csv(const std::string& filename, const bool& contains_header) {
     std::ifstream file(filename);
     if (not file.is_open()) {
         cout << "Unable to open CSV file:" << filename << endl;
@@ -269,31 +269,32 @@ bool CSV::read_csv(const std::string &filename, const bool& contains_header) {
     }
 
     // Read in the CSV file as a string and parse it
-    std::string str((std::istreambuf_iterator<char>(file)),
-                    std::istreambuf_iterator<char>());
+    std::string str((std::istreambuf_iterator<char>(file)), std::istreambuf_iterator<char>());
     return read_csv_from_string(str, contains_header);
 }
 
-void CSV::set_no_value_string(const std::string &str) { no_value_str_ = str; }
+void CSV::set_no_value_string(const std::string& str) {
+    no_value_str_ = str;
+}
 
-size_t CSV::rows() { return table_.size(); }
+size_t CSV::rows() {
+    return table_.size();
+}
 
-double CSV::at(int row, const std::string &header) {
+double CSV::at(int row, const std::string& header) {
     const int column = column_headers_.at(header);
     return table_.at(row).at(column);
 }
 
-std::list<std::string> CSV::get_csv_line_elements(const std::string &str) {
+std::list<std::string> CSV::get_csv_line_elements(const std::string& str) {
     std::list<std::string> elems;
     std::vector<std::string> tokens;
     boost::split(tokens, str, boost::is_any_of(","));
     for (unsigned int i = 0; i < tokens.size(); i++) {
         // Remove whitespace
-        tokens[i].erase(
-            std::remove_if(tokens[i].begin(),
-                           tokens[i].end(),
-                           [](unsigned char x){return std::isspace(x);}),
-            tokens[i].end());
+        tokens[i].erase(std::remove_if(tokens[i].begin(), tokens[i].end(),
+                                       [](unsigned char x) { return std::isspace(x); }),
+                        tokens[i].end());
 
         if (tokens[i] != "") {
             elems.push_back(tokens[i]);
@@ -312,19 +313,15 @@ void CSV::write_row(const int& row) {
 
 bool CSV::equals(const CSV& other) {
     if (not std::equal(column_headers_.begin(), column_headers_.end(),
-                       other.column_headers_.begin(),
-                       other.column_headers_.end())) {
+                       other.column_headers_.begin(), other.column_headers_.end())) {
         return false;
     }
 
-
-    if (not std::equal(table_.begin(), table_.end(),
-                       other.table_.begin(),
-                       other.table_.end())) {
+    if (not std::equal(table_.begin(), table_.end(), other.table_.begin(), other.table_.end())) {
         return false;
     }
 
     return true;
 }
 
-} // namespace scrimmage
+}  // namespace scrimmage
diff --git a/src/common/ColorMaps.cpp b/src/common/ColorMaps.cpp
index 094ef34a6b..7f3248601b 100644
--- a/src/common/ColorMaps.cpp
+++ b/src/common/ColorMaps.cpp
@@ -34,122 +34,121 @@
 
 namespace scrimmage {
 
-
 // Matlab's versions of JET to GRAYSCALE
 // Matlab uses the same JET version as BlueView sonar JET
 Color_t GetColor_matlab(double v, double vmin, double vmax) {
-     Color_t c = {255.0, 255.0, 255.0}; // white
-     double dv;
-
-     if (v < vmin) {
-          v = vmin;
-     } else if (v > vmax) {
-          v = vmax;
-     }
-
-     dv = vmax - vmin;
-
-     double mx = v*0.5*vmax / (0.125*vmax - vmin);
-
-     if (v < (vmin + 0.125 * dv)) {
-          c.r = vmin;
-          c.g = vmin;
-          c.b = mx + 0.5*vmax;
-     } else if (v < (vmin + 0.375 * dv)) {
-          c.r = vmin;
-          c.g = mx - 0.5*vmax;
-          c.b = vmax;
-     } else if (v < (vmin + 0.625 * dv)) {
-          c.r = mx - 1.5*vmax;
-          c.g = vmax;
-          c.b = -mx + (vmax - 1.5*(vmin-vmax));
-     } else if (v < (vmin + 0.875 * dv)) {
-          c.r = vmax;
-          c.g = -mx + (vmax - 2.5*(vmin-vmax));
-          c.b = vmin;
-     } else {
-          c.r = -mx + 4.5*vmax;
-          c.g = vmin;
-          c.b = vmin;
-     }
-     return(c);
+    Color_t c = {255.0, 255.0, 255.0};  // white
+    double dv;
+
+    if (v < vmin) {
+        v = vmin;
+    } else if (v > vmax) {
+        v = vmax;
+    }
+
+    dv = vmax - vmin;
+
+    double mx = v * 0.5 * vmax / (0.125 * vmax - vmin);
+
+    if (v < (vmin + 0.125 * dv)) {
+        c.r = vmin;
+        c.g = vmin;
+        c.b = mx + 0.5 * vmax;
+    } else if (v < (vmin + 0.375 * dv)) {
+        c.r = vmin;
+        c.g = mx - 0.5 * vmax;
+        c.b = vmax;
+    } else if (v < (vmin + 0.625 * dv)) {
+        c.r = mx - 1.5 * vmax;
+        c.g = vmax;
+        c.b = -mx + (vmax - 1.5 * (vmin - vmax));
+    } else if (v < (vmin + 0.875 * dv)) {
+        c.r = vmax;
+        c.g = -mx + (vmax - 2.5 * (vmin - vmax));
+        c.b = vmin;
+    } else {
+        c.r = -mx + 4.5 * vmax;
+        c.g = vmin;
+        c.b = vmin;
+    }
+    return (c);
 }
 
 double GetGray_matlab(Color_t c, double vmin, double vmax) {
-     double v = 0;
-     // double slope = 0.5*vmax / (0.125*vmax - vmin);
-     // double slope = (vmax - 0.5*(vmax-vmin)) / (0.125*(vmax-vmin)-vmin);
-     // double slope = 0.5*(vmax-vmin) / (0.125*(vmax-vmin)-vmin);
-     // double slope = 0.5*(vmax-vmin) / (0.125*vmax - 1.125*vmin);
-     double slope = 4;
-
-     if (c.g == vmin && c.r == vmin) {
-          // v = (c.b - 0.5*vmax) / slope;
-          v = (c.b - 0.5*(vmax-vmin)) / slope;
-     } else if (c.b == vmax && c.r == vmin) {
-          // v = (c.g + 0.5*vmax) / slope;
-          v = (c.g + 0.5*(vmax-vmin) - vmin) / slope;
-     } else if (c.g == vmax) {
-          v = (c.r + 1.5*(vmax-vmin) - vmin) / slope;
-          // v = (c.r - vmax + 5.0/2.0*(vmax-vmin)) / slope;
-     } else if (c.b == vmin && c.r == vmax) {
-          // v = (c.g - ((vmax - 2.5*(vmin-vmax)))) / (-slope);
-          v = (c.g - 3.5*(vmax-vmin) - vmin) / -slope;
-     } else if (c.g == vmin && c.b == vmin) {
-          // v = (c.r - 4.5*vmax) / (-slope);
-          v = (c.r - 4.5*(vmax) + 0.5*vmin) / -slope;
-     }
-     return v;
+    double v = 0;
+    // double slope = 0.5*vmax / (0.125*vmax - vmin);
+    // double slope = (vmax - 0.5*(vmax-vmin)) / (0.125*(vmax-vmin)-vmin);
+    // double slope = 0.5*(vmax-vmin) / (0.125*(vmax-vmin)-vmin);
+    // double slope = 0.5*(vmax-vmin) / (0.125*vmax - 1.125*vmin);
+    double slope = 4;
+
+    if (c.g == vmin && c.r == vmin) {
+        // v = (c.b - 0.5*vmax) / slope;
+        v = (c.b - 0.5 * (vmax - vmin)) / slope;
+    } else if (c.b == vmax && c.r == vmin) {
+        // v = (c.g + 0.5*vmax) / slope;
+        v = (c.g + 0.5 * (vmax - vmin) - vmin) / slope;
+    } else if (c.g == vmax) {
+        v = (c.r + 1.5 * (vmax - vmin) - vmin) / slope;
+        // v = (c.r - vmax + 5.0/2.0*(vmax-vmin)) / slope;
+    } else if (c.b == vmin && c.r == vmax) {
+        // v = (c.g - ((vmax - 2.5*(vmin-vmax)))) / (-slope);
+        v = (c.g - 3.5 * (vmax - vmin) - vmin) / -slope;
+    } else if (c.g == vmin && c.b == vmin) {
+        // v = (c.r - 4.5*vmax) / (-slope);
+        v = (c.r - 4.5 * (vmax) + 0.5 * vmin) / -slope;
+    }
+    return v;
 }
 
 // "Traditional" JET conversions
 Color_t GetColor(double v, double vmin, double vmax) {
-     Color_t c = {255.0, 255.0, 255.0}; // white
-     // Color_t c = {1.0,1.0,1.0}; // white
-     double dv;
-
-     if (v < vmin) {
-          v = vmin;
-     } else if (v > vmax) {
-          v = vmax;
-     }
-
-     dv = vmax - vmin;
-
-     if (v < (vmin + 0.25 * dv)) {
-          c.r = vmin;
-          c.g = 4.0*v*(vmax-vmin)/vmax;
-          c.b = vmax;
-     } else if (v < (vmin + 0.5 * dv)) {
-          c.r = vmin;
-          c.g = vmax;
-          c.b = 4.0*v*(vmin-vmax)/vmax-2*(vmin-vmax);
-     } else if (v < (vmin + 0.75 * dv)) {
-          c.r = 4.0*v*(vmax-vmin)/vmax-2*(vmax-vmin);
-          c.g = vmax;
-          c.b = vmin;
-     } else {
-          c.r = vmax;
-          c.g = 4.0*v*(vmin-vmax)/vmax-4.0*(vmin-vmax);
-          c.b = vmin;
-     }
-
-     return(c);
+    Color_t c = {255.0, 255.0, 255.0};  // white
+    // Color_t c = {1.0,1.0,1.0}; // white
+    double dv;
+
+    if (v < vmin) {
+        v = vmin;
+    } else if (v > vmax) {
+        v = vmax;
+    }
+
+    dv = vmax - vmin;
+
+    if (v < (vmin + 0.25 * dv)) {
+        c.r = vmin;
+        c.g = 4.0 * v * (vmax - vmin) / vmax;
+        c.b = vmax;
+    } else if (v < (vmin + 0.5 * dv)) {
+        c.r = vmin;
+        c.g = vmax;
+        c.b = 4.0 * v * (vmin - vmax) / vmax - 2 * (vmin - vmax);
+    } else if (v < (vmin + 0.75 * dv)) {
+        c.r = 4.0 * v * (vmax - vmin) / vmax - 2 * (vmax - vmin);
+        c.g = vmax;
+        c.b = vmin;
+    } else {
+        c.r = vmax;
+        c.g = 4.0 * v * (vmin - vmax) / vmax - 4.0 * (vmin - vmax);
+        c.b = vmin;
+    }
+
+    return (c);
 }
 
 int GetGray(Color_t c, double vmin, double vmax) {
-     int v = 0;
-
-     if (c.r == vmin && c.b == vmax) {
-          v = c.g * vmax / (4.0*(vmax-vmin));
-     } else if (c.r == vmin && c.g == vmax) {
-          v = (c.b + 2*(vmin-vmax)) / (4.0*(vmin-vmax)/vmax);
-     } else if (c.g == vmax && c.b == vmin) {
-          v = (c.r + 2*(vmax-vmin)) / (4.0*(vmax-vmin)/vmax);
-     } else {
-          v = (c.g + 4.0*(vmin-vmax)) / (4.0*(vmin-vmax)/vmax);
-     }
-     return v;
+    int v = 0;
+
+    if (c.r == vmin && c.b == vmax) {
+        v = c.g * vmax / (4.0 * (vmax - vmin));
+    } else if (c.r == vmin && c.g == vmax) {
+        v = (c.b + 2 * (vmin - vmax)) / (4.0 * (vmin - vmax) / vmax);
+    } else if (c.g == vmax && c.b == vmin) {
+        v = (c.r + 2 * (vmax - vmin)) / (4.0 * (vmax - vmin) / vmax);
+    } else {
+        v = (c.g + 4.0 * (vmin - vmax)) / (4.0 * (vmin - vmax) / vmax);
+    }
+    return v;
 }
 
-} // namespace scrimmage
+}  // namespace scrimmage
diff --git a/src/common/DelayedTask.cpp b/src/common/DelayedTask.cpp
index 1631426183..c144dddd28 100644
--- a/src/common/DelayedTask.cpp
+++ b/src/common/DelayedTask.cpp
@@ -36,18 +36,19 @@
 
 namespace scrimmage {
 
-DelayedTask::DelayedTask() : DelayedTask(0, -1) {}
+DelayedTask::DelayedTask() : DelayedTask(0, -1) {
+}
 
-DelayedTask::DelayedTask(double _delay, int repeats) :
-        disable(false),
-        delay(_delay),
-        last_updated_time(-std::numeric_limits<double>::infinity()),
-        end_time(std::numeric_limits<double>::infinity()),
-        eps(0.0),
-        condition(nullptr),
-        task(nullptr),
-        repeat_infinitely_(true),
-        repeats_left_(-1) {
+DelayedTask::DelayedTask(double _delay, int repeats)
+    : disable(false),
+      delay(_delay),
+      last_updated_time(-std::numeric_limits<double>::infinity()),
+      end_time(std::numeric_limits<double>::infinity()),
+      eps(0.0),
+      condition(nullptr),
+      task(nullptr),
+      repeat_infinitely_(true),
+      repeats_left_(-1) {
     set_repeats(repeats);
 }
 
@@ -88,4 +89,4 @@ void DelayedTask::set_repeat_infinitely(bool repeat_infinitely) {
     repeats_left_ = -1;
 }
 
-} // namespace scrimmage
+}  // namespace scrimmage
diff --git a/src/common/ExponentialFilter.cpp b/src/common/ExponentialFilter.cpp
index fcd9387db1..5a4a9f9090 100644
--- a/src/common/ExponentialFilter.cpp
+++ b/src/common/ExponentialFilter.cpp
@@ -37,13 +37,14 @@
 
 namespace scrimmage {
 
-ExponentialFilter::ExponentialFilter() :
-    ExponentialFilter(1.0) {}
+ExponentialFilter::ExponentialFilter() : ExponentialFilter(1.0) {
+}
 
-ExponentialFilter::ExponentialFilter(double time_constant) :
-    estimate_(0),
-    time_last_estimate_(-std::numeric_limits<double>::infinity()),
-    time_constant_(time_constant) {}
+ExponentialFilter::ExponentialFilter(double time_constant)
+    : estimate_(0),
+      time_last_estimate_(-std::numeric_limits<double>::infinity()),
+      time_constant_(time_constant) {
+}
 
 double ExponentialFilter::get_estimate() const {
     return estimate_;
@@ -63,4 +64,4 @@ double ExponentialFilter::add_estimate(double estimate, double t) {
     return estimate_;
 }
 
-} // namespace scrimmage
+}  // namespace scrimmage
diff --git a/src/common/FileSearch.cpp b/src/common/FileSearch.cpp
index b0ec53092d..71db1b0585 100644
--- a/src/common/FileSearch.cpp
+++ b/src/common/FileSearch.cpp
@@ -33,17 +33,16 @@
 #include <scrimmage/common/FileSearch.h>
 #include <scrimmage/parse/ParseUtils.h>
 
-#include <iostream>
-#include <unordered_set>
-
 #include <boost/algorithm/string/predicate.hpp>
 #include <boost/filesystem.hpp>
 #include <boost/filesystem/path.hpp>
+#include <boost/optional.hpp>
+#include <boost/range/adaptor/uniqued.hpp>
 #include <boost/range/algorithm/find_if.hpp>
 #include <boost/range/algorithm/sort.hpp>
-#include <boost/range/adaptor/uniqued.hpp>
 #include <boost/tokenizer.hpp>
-#include <boost/optional.hpp>
+#include <iostream>
+#include <unordered_set>
 
 namespace fs = ::boost::filesystem;
 namespace ba = boost::adaptors;
@@ -51,10 +50,11 @@ namespace br = boost::range;
 
 namespace scrimmage {
 
-void FileSearch::clear() {cache_.clear();}
+void FileSearch::clear() {
+    cache_.clear();
+}
 
-boost::optional<std::string> FileSearch::find_mission(std::string mission,
-        bool verbose) {
+boost::optional<std::string> FileSearch::find_mission(std::string mission, bool verbose) {
     if (!boost::algorithm::ends_with(mission, "xml")) {
         mission = mission + ".xml";
     }
@@ -62,20 +62,15 @@ boost::optional<std::string> FileSearch::find_mission(std::string mission,
     if (fs::exists(mission)) return mission;
 
     std::string out;
-    auto search = [&](auto env) {return this->find_file(
-            mission, "xml", env, out, verbose);};
-    std::list<std::string> env_vars
-        {"SCRIMMAGE_MISSION_PATH",
-            "/usr/share/scrimmage",
-            "/usr/local/share/scrimmage"};
+    auto search = [&](auto env) { return this->find_file(mission, "xml", env, out, verbose); };
+    std::list<std::string> env_vars{"SCRIMMAGE_MISSION_PATH", "/usr/share/scrimmage",
+                                    "/usr/local/share/scrimmage"};
     auto it = br::find_if(env_vars, search);
-    return it ==
-        env_vars.end() ? boost::none : boost::optional<std::string>(out);
+    return it == env_vars.end() ? boost::none : boost::optional<std::string>(out);
 }
 
-bool FileSearch::find_file(const std::string &search,
-        std::string ext, const std::string &env_var, std::string &result,
-        bool verbose) {
+bool FileSearch::find_file(const std::string& search, std::string ext, const std::string& env_var,
+                           std::string& result, bool verbose) {
     // Find the xml file.
     // Search order:
     // 1. search could be the full path
@@ -102,15 +97,14 @@ bool FileSearch::find_file(const std::string &search,
 
     if (!fs::exists(search)) {
         // files[search_filename] = list of full paths
-        dbg(std::string("not an absolute path, checking recursively in ")
-                + env_var);
+        dbg(std::string("not an absolute path, checking recursively in ") + env_var);
         std::unordered_map<std::string, std::list<std::string>> files;
         char native_path_sep = fs::path("/").make_preferred().native().at(0);
         std::size_t last_seperator = search_filename.find_last_of(native_path_sep);
-        if(last_seperator != std::string::npos) {
-          search_filename = search_filename.substr(last_seperator + 1);
+        if (last_seperator != std::string::npos) {
+            search_filename = search_filename.substr(last_seperator + 1);
         }
-        
+
         find_files(env_var, ext, files, verbose);
         filenames = files[search_filename];
     } else {
@@ -126,25 +120,22 @@ bool FileSearch::find_file(const std::string &search,
     result = filenames.back();
 
     if (filenames.size() > 1) {
-        std::cout <<
-            "===============================================" << std::endl;
-        std::cout <<
-            "WARNING: Multiple XML files with same name found" << std::endl;
-        for (std::string &full_path : filenames) {
+        std::cout << "===============================================" << std::endl;
+        std::cout << "WARNING: Multiple XML files with same name found" << std::endl;
+        for (std::string& full_path : filenames) {
             std::cout << full_path << std::endl;
         }
         std::cout << "Using XML file at: " << result << std::endl;
-        std::cout <<
-            "===============================================" << std::endl;
+        std::cout << "===============================================" << std::endl;
     }
     return true;
 }
 
 // Give an environment variable and a file extension:
 // Find the absolute path to all files in environment variable paths
-void FileSearch::find_files(std::string env_var, const std::string &ext,
-        std::unordered_map<std::string, std::list<std::string>> &out,
-        bool verbose) {
+void FileSearch::find_files(std::string env_var, const std::string& ext,
+                            std::unordered_map<std::string, std::list<std::string>>& out,
+                            bool verbose) {
     auto dbg = [&](std::string msg) {
         if (verbose) std::cout << "find_files: " << msg << std::endl;
     };
@@ -171,8 +162,7 @@ void FileSearch::find_files(std::string env_var, const std::string &ext,
     if (env_var.find("/") == std::string::npos) {
         const char* env_p = std::getenv(env_var.c_str());
         if (env_p == NULL) {
-            std::cout << env_var <<
-                " environment variable not set" << std::endl;
+            std::cout << env_var << " environment variable not set" << std::endl;
             return;
         }
 
@@ -186,7 +176,7 @@ void FileSearch::find_files(std::string env_var, const std::string &ext,
 
     // Tokenize the path and loop through each directory
     boost::char_separator<char> sep(":");
-    boost::tokenizer< boost::char_separator<char> > tokens(env_path, sep);
+    boost::tokenizer<boost::char_separator<char>> tokens(env_path, sep);
 
     // we are checking recursively anyway so remove
     // more specific directories
@@ -197,19 +187,17 @@ void FileSearch::find_files(std::string env_var, const std::string &ext,
     char native_path_sep = fs::path("/").make_preferred().native().at(0);
 
     while (it != tok.end()) {
-        auto starts_with = [&](std::string &s) {
-            return (*it == s) ||
-                (boost::starts_with(s, *it) &&
-                s.size() > it->size() &&
-                s.at(it->size()) == native_path_sep);
+        auto starts_with = [&](std::string& s) {
+            return (*it == s)
+                   || (boost::starts_with(s, *it) && s.size() > it->size()
+                       && s.at(it->size()) == native_path_sep);
         };
-        tok.erase(std::remove_if(std::next(it), tok.end(), starts_with),
-                tok.end());
+        tok.erase(std::remove_if(std::next(it), tok.end(), starts_with), tok.end());
         it++;
     }
 
     dbg(std::string("not found in cache, looping recursively in ") + env_path);
-    for (const std::string &t : tok) {
+    for (const std::string& t : tok) {
         // Search for all files in the current directory with
         // the extension
         fs::path root = t;
diff --git a/src/common/GlobalService.cpp b/src/common/GlobalService.cpp
index 8cb6f2e750..ba3f07bec5 100644
--- a/src/common/GlobalService.cpp
+++ b/src/common/GlobalService.cpp
@@ -31,8 +31,8 @@
 
 #include <scrimmage/common/GlobalService.h>
 
-#include <string>
 #include <algorithm>
+#include <string>
 
 namespace scrimmage {
 
@@ -45,24 +45,25 @@ GlobalService::~GlobalService() {
     services_.clear();
 }
 
-std::unordered_map<std::string, Service> &GlobalService::services() {return services_;}
+std::unordered_map<std::string, Service>& GlobalService::services() {
+    return services_;
+}
 
-bool GlobalService::call_service(scrimmage::MessageBasePtr req,
-        scrimmage::MessageBasePtr &res, const std::string &service_name) {
+bool GlobalService::call_service(scrimmage::MessageBasePtr req, scrimmage::MessageBasePtr& res,
+                                 const std::string& service_name) {
     auto it = services_.find(service_name);
     if (it == services_.end()) {
-        std::cout << "request for global service ("
-            << service_name
-            << ") that does not exist" << std::endl;
+        std::cout << "request for global service (" << service_name << ") that does not exist"
+                  << std::endl;
         std::cout << "Global services are: ";
-        for (auto &kv : services_) {
+        for (auto& kv : services_) {
             std::cout << kv.first << ", ";
         }
         std::cout << std::endl;
         return false;
     }
 
-    Service &service = it->second;
+    Service& service = it->second;
     bool success = service(req, res);
 
     if (!success) {
diff --git a/src/common/ID.cpp b/src/common/ID.cpp
index fe78a1e614..7332b59df9 100644
--- a/src/common/ID.cpp
+++ b/src/common/ID.cpp
@@ -36,30 +36,42 @@
 
 namespace scrimmage {
 
-ID::ID() : id_(0), sub_swarm_id_(0), team_id_(0) {}
+ID::ID() : id_(0), sub_swarm_id_(0), team_id_(0) {
+}
 
-ID::ID(int id, int sub_swarm_id, int team_id) :
-    id_(id), sub_swarm_id_(sub_swarm_id), team_id_(team_id) {}
+ID::ID(int id, int sub_swarm_id, int team_id)
+    : id_(id), sub_swarm_id_(sub_swarm_id), team_id_(team_id) {
+}
 
-void ID::set_id(int id) { id_ = id; }
+void ID::set_id(int id) {
+    id_ = id;
+}
 
-void ID::set_sub_swarm_id(int sub_swarm_id) {sub_swarm_id_ = sub_swarm_id; }
+void ID::set_sub_swarm_id(int sub_swarm_id) {
+    sub_swarm_id_ = sub_swarm_id;
+}
 
-void ID::set_team_id(int team_id) { team_id_ = team_id; }
+void ID::set_team_id(int team_id) {
+    team_id_ = team_id;
+}
 
-int ID::id() const { return id_; }
+int ID::id() const {
+    return id_;
+}
 
-int ID::sub_swarm_id() const { return sub_swarm_id_; }
+int ID::sub_swarm_id() const {
+    return sub_swarm_id_;
+}
 
-int ID::team_id() const { return team_id_; }
+int ID::team_id() const {
+    return team_id_;
+}
 
-bool ID::operator==(const ID &other) const {
-    return id_ == other.id_ &&
-        sub_swarm_id_ == other.sub_swarm_id_ &&
-        team_id_ == other.team_id_;
+bool ID::operator==(const ID& other) const {
+    return id_ == other.id_ && sub_swarm_id_ == other.sub_swarm_id_ && team_id_ == other.team_id_;
 }
 
-bool ID::operator<(const ID &other) const {
+bool ID::operator<(const ID& other) const {
     if (id_ < other.id_) {
         return true;
     } else if (sub_swarm_id_ < other.sub_swarm_id_) {
@@ -71,10 +83,9 @@ bool ID::operator<(const ID &other) const {
     }
 }
 
-std::ostream &operator<<(std::ostream &os, const ID &id) {
+std::ostream& operator<<(std::ostream& os, const ID& id) {
     os << id.id_ << ", " << id.sub_swarm_id_ << ", " << id.team_id_;
     return os;
 }
 
-} // namespace scrimmage
-
+}  // namespace scrimmage
diff --git a/src/common/PID.cpp b/src/common/PID.cpp
index a96b536cf7..39d3511c16 100644
--- a/src/common/PID.cpp
+++ b/src/common/PID.cpp
@@ -30,26 +30,36 @@
  *
  */
 
-#include <scrimmage/math/Angles.h>
 #include <scrimmage/common/PID.h>
+#include <scrimmage/math/Angles.h>
 
-#include <cmath>
 #include <algorithm>
-#include <vector>
-
-#include <boost/algorithm/string.hpp>
 #include <boost/algorithm/clamp.hpp>
+#include <boost/algorithm/string.hpp>
+#include <cmath>
+#include <vector>
 
 using boost::algorithm::clamp;
 
 namespace scrimmage {
 
-PID::PID() : kp_(0), ki_(0), kd_(0), prev_error_(0), integral_(0),
-             setpoint_(0), integral_band_(0), is_angle_(false),
-             derivative_(0), error_(0), output_min_(0), output_max_(0),
-             enable_output_limits_(false) {}
+PID::PID()
+    : kp_(0),
+      ki_(0),
+      kd_(0),
+      prev_error_(0),
+      integral_(0),
+      setpoint_(0),
+      integral_band_(0),
+      is_angle_(false),
+      derivative_(0),
+      error_(0),
+      output_min_(0),
+      output_max_(0),
+      enable_output_limits_(false) {
+}
 
-bool PID::init(const std::string &str, const bool &is_angle) {
+bool PID::init(const std::string& str, const bool& is_angle) {
     std::vector<std::string> str_vals;
     boost::split(str_vals, str, boost::is_any_of(","));
 
@@ -73,24 +83,23 @@ bool PID::init(const std::string &str, const bool &is_angle) {
     return true;
 }
 
-void PID::set_output_limits(const double &min, const double &max,
-                            const double &enable) {
+void PID::set_output_limits(const double& min, const double& max, const double& enable) {
     output_min_ = min;
     output_max_ = max;
     enable_output_limits_ = enable;
 }
 
-void PID::set_parameters(const double &kp, const double &ki, const double &kd) {
+void PID::set_parameters(const double& kp, const double& ki, const double& kd) {
     kp_ = kp;
     ki_ = ki;
     kd_ = kd;
 }
 
-void PID::set_setpoint(const double &setpoint) {
+void PID::set_setpoint(const double& setpoint) {
     setpoint_ = setpoint;
 }
 
-void PID::set_integral_band(const double &integral_band) {
+void PID::set_integral_band(const double& integral_band) {
     integral_band_ = integral_band;
 }
 
@@ -100,19 +109,39 @@ void PID::reset() {
     derivative_ = 0.0;
 }
 
-void PID::set_is_angle(const bool &is_angle) { is_angle_ = is_angle; }
+void PID::set_is_angle(const bool& is_angle) {
+    is_angle_ = is_angle;
+}
 
-const double & PID::setpoint() { return setpoint_; }
-const double & PID::kp() { return kp_; }
-const double & PID::ki() { return ki_; }
-const double & PID::kd() { return kd_; }
-const double & PID::integral_band() { return integral_band_; }
-const double & PID::integral() { return integral_; }
-const double & PID::error() { return error_; }
-const double & PID::previous_error() { return prev_error_; }
-const double & PID::derivative() { return derivative_; }
+const double& PID::setpoint() {
+    return setpoint_;
+}
+const double& PID::kp() {
+    return kp_;
+}
+const double& PID::ki() {
+    return ki_;
+}
+const double& PID::kd() {
+    return kd_;
+}
+const double& PID::integral_band() {
+    return integral_band_;
+}
+const double& PID::integral() {
+    return integral_;
+}
+const double& PID::error() {
+    return error_;
+}
+const double& PID::previous_error() {
+    return prev_error_;
+}
+const double& PID::derivative() {
+    return derivative_;
+}
 
-double PID::step(const double &dt, const double &measurement) {
+double PID::step(const double& dt, const double& measurement) {
     error_ = setpoint_ - measurement;
     if (is_angle_) {
         error_ = Angles::angle_pi(error_);
@@ -125,9 +154,9 @@ double PID::step(const double &dt, const double &measurement) {
     }
 
     derivative_ = (error_ - prev_error_) / std::max(1.0e-9, dt);
-    double u = kp_*error_ + ki_*integral_ + kd_*derivative_;
+    double u = kp_ * error_ + ki_ * integral_ + kd_ * derivative_;
     prev_error_ = error_;
 
     return enable_output_limits_ ? clamp(u, output_min_, output_max_) : u;
 }
-} // namespace scrimmage
+}  // namespace scrimmage
diff --git a/src/common/ParameterServer.cpp b/src/common/ParameterServer.cpp
index 4b37dbe74b..fdd84bc7b0 100644
--- a/src/common/ParameterServer.cpp
+++ b/src/common/ParameterServer.cpp
@@ -32,30 +32,26 @@
 
 #include <scrimmage/common/ParameterServer.h>
 
-#include <string>
 #include <algorithm>
+#include <string>
 
 namespace scrimmage {
 void ParameterServer::unregister_params(PluginPtr owner) {
     // For all parameters, remove all parameters owned by this plugin
-    for (auto &kv1 : params_) {
-        for (auto &kv2 : kv1.second) {
+    for (auto& kv1 : params_) {
+        for (auto& kv2 : kv1.second) {
             remove_if_owner(kv2.second, owner);
         }
     }
 }
 
-bool ParameterServer::remove_if_owner(std::set<ParameterBasePtr> &param_set,
-                                      PluginPtr owner) {
-    auto it_param = std::find_if(param_set.begin(),
-                                 param_set.end(),
-                                 [&](auto p) {
-                                     return p->owner() == owner;
-                                 });
+bool ParameterServer::remove_if_owner(std::set<ParameterBasePtr>& param_set, PluginPtr owner) {
+    auto it_param = std::find_if(param_set.begin(), param_set.end(),
+                                 [&](auto p) { return p->owner() == owner; });
     if (it_param != param_set.end()) {
         param_set.erase(it_param);
         return true;
     }
     return false;
 }
-} // namespace scrimmage
+}  // namespace scrimmage
diff --git a/src/common/RTree.cpp b/src/common/RTree.cpp
index 58073dd85d..81c02b73d8 100644
--- a/src/common/RTree.cpp
+++ b/src/common/RTree.cpp
@@ -33,10 +33,9 @@
 #include <scrimmage/common/RTree.h>
 
 #include <algorithm>
-#include <list>
-
 #include <boost/geometry.hpp>
 #include <boost/geometry/index/rtree.hpp>
+#include <list>
 
 namespace bg = boost::geometry;
 namespace bgi = boost::geometry::index;
@@ -58,7 +57,7 @@ void RTree::clear() {
     rtree_team_.clear();
 }
 
-void RTree::add(const Eigen::Vector3d &pos, const ID &id) {
+void RTree::add(const Eigen::Vector3d& pos, const ID& id) {
     point p(pos(0), pos(1), pos(2));
     std::pair<point, ID> pair(p, id);
     rtree_->insert(pair);
@@ -72,27 +71,24 @@ void RTree::add(const Eigen::Vector3d &pos, const ID &id) {
     it->second->insert(pair);
 }
 
-void results_to_neighbors(std::list<point_id_t> &results,
-                          std::vector<ID> &neighbors,
-                          int self_id) {
+void results_to_neighbors(std::list<point_id_t>& results, std::vector<ID>& neighbors, int self_id) {
     neighbors.clear();
     neighbors.reserve(results.size());
     if (self_id >= 0) {
-        for (point_id_t &pt_id : results) {
+        for (point_id_t& pt_id : results) {
             if (pt_id.second.id() != self_id) {
                 neighbors.push_back(pt_id.second);
             }
         }
     } else {
-        for (point_id_t &pt_id : results) {
+        for (point_id_t& pt_id : results) {
             neighbors.push_back(pt_id.second);
         }
     }
 }
 
-void RTree::nearest_n_neighbors(const Eigen::Vector3d &pos,
-                                std::vector<ID> &neighbors, unsigned int n,
-                                int self_id, int team_id) const {
+void RTree::nearest_n_neighbors(const Eigen::Vector3d& pos, std::vector<ID>& neighbors,
+                                unsigned int n, int self_id, int team_id) const {
     std::list<point_id_t> results;
     point sought(pos(0), pos(1), pos(2));
 
@@ -116,9 +112,7 @@ void RTree::nearest_n_neighbors(const Eigen::Vector3d &pos,
     results_to_neighbors(results, neighbors, self_id);
 }
 
-void RTree::neighbors_in_range(const Eigen::Vector3d &pos,
-                               std::vector<ID> &neighbors,
-                               double dist,
+void RTree::neighbors_in_range(const Eigen::Vector3d& pos, std::vector<ID>& neighbors, double dist,
                                int self_id, int team_id) const {
     // see here: http://stackoverflow.com/a/22910447
     std::list<point_id_t> results;
@@ -127,31 +121,25 @@ void RTree::neighbors_in_range(const Eigen::Vector3d &pos,
     double z = pos(2);
     point sought(x, y, z);
 
-    bg::model::box<point> box(
-        point(x - dist, y - dist, z - dist), point(x + dist, y + dist, z + dist)
-    );
+    bg::model::box<point> box(point(x - dist, y - dist, z - dist),
+                              point(x + dist, y + dist, z + dist));
 
-    auto dist_func = [&](point_id_t const& v) {return bg::distance(v.first, sought) < dist;};
+    auto dist_func = [&](point_id_t const& v) { return bg::distance(v.first, sought) < dist; };
 
     if (team_id == -1) {
-        rtree_->query(
-            bgi::within(box) && bgi::satisfies(dist_func),
-            std::front_inserter(results)
-        );
+        rtree_->query(bgi::within(box) && bgi::satisfies(dist_func), std::front_inserter(results));
     } else {
         auto it = rtree_team_.find(team_id);
         if (it == rtree_team_.end()) {
             neighbors.clear();
             return;
         } else {
-            it->second->query(
-                bgi::within(box) && bgi::satisfies(dist_func),
-                std::front_inserter(results)
-            );
+            it->second->query(bgi::within(box) && bgi::satisfies(dist_func),
+                              std::front_inserter(results));
         }
     }
 
     results_to_neighbors(results, neighbors, self_id);
 }
 
-} // namespace scrimmage
+}  // namespace scrimmage
diff --git a/src/common/Random.cpp b/src/common/Random.cpp
index d5d539d4b9..91cff03475 100644
--- a/src/common/Random.cpp
+++ b/src/common/Random.cpp
@@ -31,16 +31,21 @@
  */
 
 #include <scrimmage/common/Random.h>
-#include <chrono> // NOLINT
+
+#include <chrono>  // NOLINT
 
 namespace scrimmage {
 
-Random::Random() :
-    seed_(std::chrono::system_clock::now().time_since_epoch().count()),
-    gener_(std::make_shared<std::default_random_engine>()),
-    rng_normal_(0, 1), rng_uniform_(-1, 1) {}
+Random::Random()
+    : seed_(std::chrono::system_clock::now().time_since_epoch().count()),
+      gener_(std::make_shared<std::default_random_engine>()),
+      rng_normal_(0, 1),
+      rng_uniform_(-1, 1) {
+}
 
-uint32_t Random::get_seed() {return seed_;}
+uint32_t Random::get_seed() {
+    return seed_;
+}
 
 void Random::seed() {
     seed(std::chrono::system_clock::now().time_since_epoch().count());
@@ -72,14 +77,14 @@ int Random::rng_uniform_int(int low, int high) {
     return std::uniform_int_distribution<int>(low, high)(*gener_);
 }
 
-int Random::rng_discrete_int(std::vector<double> &weights) {
+int Random::rng_discrete_int(std::vector<double>& weights) {
     std::discrete_distribution<int> dist(weights.begin(), weights.end());
     return dist(*gener_);
 }
 
-std::shared_ptr<std::normal_distribution<double>>
-Random::make_rng_normal(double mean, double sigma) {
+std::shared_ptr<std::normal_distribution<double>> Random::make_rng_normal(double mean,
+                                                                          double sigma) {
     return std::make_shared<std::normal_distribution<double>>(mean, sigma);
 }
 
-} // namespace scrimmage
+}  // namespace scrimmage
diff --git a/src/common/Shape.cpp b/src/common/Shape.cpp
index 8fdd9b13d7..9fd046d0e6 100644
--- a/src/common/Shape.cpp
+++ b/src/common/Shape.cpp
@@ -31,8 +31,8 @@
  */
 
 #include <scrimmage/common/Shape.h>
-#include <scrimmage/proto/Shape.pb.h>
 #include <scrimmage/proto/ProtoConversions.h>
+#include <scrimmage/proto/Shape.pb.h>
 
 #include <iostream>
 #include <vector>
@@ -43,8 +43,7 @@ namespace sp = scrimmage_proto;
 namespace scrimmage {
 namespace shape {
 
-ShapePtr make_shape(const Eigen::Vector3d &color,
-                    const double &opacity) {
+ShapePtr make_shape(const Eigen::Vector3d& color, const double& opacity) {
     auto shape = std::make_shared<scrimmage_proto::Shape>();
     sc::set(shape->mutable_color(), color);
     shape->set_opacity(opacity);
@@ -53,12 +52,9 @@ ShapePtr make_shape(const Eigen::Vector3d &color,
     return shape;
 }
 
-ShapePtr make_arc(const Eigen::Vector3d &center,
-                  const scrimmage::Quaternion &quat,
-                  const double &radius,
-                  const double &angle,
-                  const Eigen::Vector3d &color,
-                  const double &opacity) {
+ShapePtr make_arc(const Eigen::Vector3d& center, const scrimmage::Quaternion& quat,
+                  const double& radius, const double& angle, const Eigen::Vector3d& color,
+                  const double& opacity) {
     auto shape = make_shape(color, opacity);
     sc::set(shape->mutable_arc()->mutable_circle()->mutable_center(), center);
     sc::set(shape->mutable_arc()->mutable_circle()->mutable_quat(), quat);
@@ -67,11 +63,8 @@ ShapePtr make_arc(const Eigen::Vector3d &center,
     return shape;
 }
 
-ShapePtr make_arrow(const Eigen::Vector3d &pos,
-                    const scrimmage::Quaternion &quat,
-                    const double &length,
-                    const Eigen::Vector3d &color,
-                    const double &opacity) {
+ShapePtr make_arrow(const Eigen::Vector3d& pos, const scrimmage::Quaternion& quat,
+                    const double& length, const Eigen::Vector3d& color, const double& opacity) {
     auto shape = make_shape(color, opacity);
     Eigen::Vector3d head = pos + quat * (Eigen::Vector3d::UnitX() * length);
     sc::set(shape->mutable_arrow()->mutable_tail(), pos);
@@ -79,11 +72,8 @@ ShapePtr make_arrow(const Eigen::Vector3d &pos,
     return shape;
 }
 
-ShapePtr make_circle(const Eigen::Vector3d &center,
-                     const scrimmage::Quaternion &quat,
-                     const double &radius,
-                     const Eigen::Vector3d &color,
-                     const double &opacity) {
+ShapePtr make_circle(const Eigen::Vector3d& center, const scrimmage::Quaternion& quat,
+                     const double& radius, const Eigen::Vector3d& color, const double& opacity) {
     auto shape = make_shape(color, opacity);
     sc::set(shape->mutable_circle()->mutable_center(), center);
     sc::set(shape->mutable_circle()->mutable_quat(), quat);
@@ -92,12 +82,9 @@ ShapePtr make_circle(const Eigen::Vector3d &center,
     return shape;
 }
 
-ShapePtr make_cone(const Eigen::Vector3d &direction,
-                   const Eigen::Vector3d &apex,
-                   const double &height,
-                   const double &base_radius,
-                   const Eigen::Vector3d &color,
-                   const double &opacity) {
+ShapePtr make_cone(const Eigen::Vector3d& direction, const Eigen::Vector3d& apex,
+                   const double& height, const double& base_radius, const Eigen::Vector3d& color,
+                   const double& opacity) {
     auto shape = make_shape(color, opacity);
     sc::set(shape->mutable_cone()->mutable_direction(), direction);
     sc::set(shape->mutable_cone()->mutable_apex(), apex);
@@ -106,13 +93,9 @@ ShapePtr make_cone(const Eigen::Vector3d &direction,
     return shape;
 }
 
-ShapePtr make_cuboid(const Eigen::Vector3d &center,
-                     const scrimmage::Quaternion &quat,
-                     const double &x_length,
-                     const double &y_length,
-                     const double &z_length,
-                     const Eigen::Vector3d &color,
-                     const double &opacity) {
+ShapePtr make_cuboid(const Eigen::Vector3d& center, const scrimmage::Quaternion& quat,
+                     const double& x_length, const double& y_length, const double& z_length,
+                     const Eigen::Vector3d& color, const double& opacity) {
     auto shape = make_shape(color, opacity);
     shape->mutable_cuboid()->set_x_length(x_length);
     shape->mutable_cuboid()->set_y_length(y_length);
@@ -122,12 +105,9 @@ ShapePtr make_cuboid(const Eigen::Vector3d &center,
     return shape;
 }
 
-ShapePtr make_ellipse(const Eigen::Vector3d &center,
-                      const scrimmage::Quaternion &quat,
-                      const double &x_radius,
-                      const double &y_radius,
-                      const Eigen::Vector3d &color,
-                      const double &opacity) {
+ShapePtr make_ellipse(const Eigen::Vector3d& center, const scrimmage::Quaternion& quat,
+                      const double& x_radius, const double& y_radius, const Eigen::Vector3d& color,
+                      const double& opacity) {
     auto shape = make_shape(color, opacity);
     sc::set(shape->mutable_ellipse()->mutable_center(), center);
     sc::set(shape->mutable_ellipse()->mutable_quat(), quat);
@@ -136,22 +116,17 @@ ShapePtr make_ellipse(const Eigen::Vector3d &center,
     return shape;
 }
 
-ShapePtr make_line(const Eigen::Vector3d &start,
-                   const Eigen::Vector3d &stop,
-                   const Eigen::Vector3d &color,
-                   const double &opacity) {
+ShapePtr make_line(const Eigen::Vector3d& start, const Eigen::Vector3d& stop,
+                   const Eigen::Vector3d& color, const double& opacity) {
     auto shape = make_shape(color, opacity);
     sc::set(shape->mutable_line()->mutable_start(), start);
     sc::set(shape->mutable_line()->mutable_end(), stop);
     return shape;
 }
 
-ShapePtr make_mesh(const std::string &name,
-                   const Eigen::Vector3d &center,
-                   const scrimmage::Quaternion &quat,
-                   const double &scale,
-                   const Eigen::Vector3d &color,
-                   const double &opacity) {
+ShapePtr make_mesh(const std::string& name, const Eigen::Vector3d& center,
+                   const scrimmage::Quaternion& quat, const double& scale,
+                   const Eigen::Vector3d& color, const double& opacity) {
     auto shape = make_shape(color, opacity);
     shape->mutable_mesh()->set_name(name);
     sc::set(shape->mutable_mesh()->mutable_center(), center);
@@ -160,14 +135,10 @@ ShapePtr make_mesh(const std::string &name,
     return shape;
 }
 
-ShapePtr make_plane(const Eigen::Vector3d &center,
-                    const scrimmage::Quaternion &quat,
-                    const double &x_length,
-                    const double &y_length,
-                    const std::string &texture,
-                    const bool &diffuse_lighting,
-                    const Eigen::Vector3d &color,
-                    const double &opacity) {
+ShapePtr make_plane(const Eigen::Vector3d& center, const scrimmage::Quaternion& quat,
+                    const double& x_length, const double& y_length, const std::string& texture,
+                    const bool& diffuse_lighting, const Eigen::Vector3d& color,
+                    const double& opacity) {
     auto shape = make_shape(color, opacity);
     sc::set(shape->mutable_plane()->mutable_center(), center);
     sc::set(shape->mutable_plane()->mutable_quat(), quat);
@@ -178,45 +149,40 @@ ShapePtr make_plane(const Eigen::Vector3d &center,
     return shape;
 }
 
-ShapePtr make_pointcloud(const std::list<Eigen::Vector3d> &points,
-                         const std::list<Eigen::Vector3d> &point_colors,
-                         const double &size,
-                         const Eigen::Vector3d &color,
-                         const double &opacity) {
+ShapePtr make_pointcloud(const std::list<Eigen::Vector3d>& points,
+                         const std::list<Eigen::Vector3d>& point_colors, const double& size,
+                         const Eigen::Vector3d& color, const double& opacity) {
     auto shape = make_shape(color, opacity);
     shape->mutable_pointcloud()->set_size(size);
-    for (const Eigen::Vector3d &p : points) {
-        auto *point = shape->mutable_pointcloud()->add_point();
+    for (const Eigen::Vector3d& p : points) {
+        auto* point = shape->mutable_pointcloud()->add_point();
         sc::set(point, p);
     }
     return shape;
 }
 
-ShapePtr make_polydata(const std::list<Eigen::Vector3d> &points,
-                       const Eigen::Vector3d &color,
-                       const double &opacity) {
+ShapePtr make_polydata(const std::list<Eigen::Vector3d>& points, const Eigen::Vector3d& color,
+                       const double& opacity) {
     auto shape = make_shape(color, opacity);
-    for (auto &point : points) {
+    for (auto& point : points) {
         auto p = shape->mutable_polydata()->add_point();
         sc::set(p, point);
     }
     return shape;
 }
 
-ShapePtr make_polygon(const std::list<Eigen::Vector3d> &points,
-                      const Eigen::Vector3d &color,
-                      const double &opacity) {
+ShapePtr make_polygon(const std::list<Eigen::Vector3d>& points, const Eigen::Vector3d& color,
+                      const double& opacity) {
     auto shape = make_shape(color, opacity);
-    for (auto &point : points) {
+    for (auto& point : points) {
         auto p = shape->mutable_polygon()->add_point();
         sc::set(p, point);
     }
     return shape;
 }
 
-ShapePtr make_polyhedron(const std::list<Eigen::Vector3d> &points,
-                         const Eigen::Vector3d &color,
-                         const double &opacity) {
+ShapePtr make_polyhedron(const std::list<Eigen::Vector3d>& points, const Eigen::Vector3d& color,
+                         const double& opacity) {
     auto shape = make_shape(color, opacity);
     for (auto point : points) {
         auto p = shape->mutable_polyhedron()->add_point();
@@ -225,30 +191,26 @@ ShapePtr make_polyhedron(const std::list<Eigen::Vector3d> &points,
     return shape;
 }
 
-ShapePtr make_polyline(const std::list<Eigen::Vector3d> &points,
-                       const Eigen::Vector3d &color,
-                       const double &opacity) {
+ShapePtr make_polyline(const std::list<Eigen::Vector3d>& points, const Eigen::Vector3d& color,
+                       const double& opacity) {
     auto shape = make_shape(color, opacity);
-    for (auto &point : points) {
+    for (auto& point : points) {
         auto p = shape->mutable_polyline()->add_point();
         sc::set(p, point);
     }
     return shape;
 }
 
-ShapePtr make_sphere(const Eigen::Vector3d &center,
-                     const double &radius,
-                     const Eigen::Vector3d &color,
-                     const double &opacity) {
+ShapePtr make_sphere(const Eigen::Vector3d& center, const double& radius,
+                     const Eigen::Vector3d& color, const double& opacity) {
     auto shape = make_shape(color, opacity);
     shape->mutable_sphere()->set_radius(radius);
     sc::set(shape->mutable_sphere()->mutable_center(), center);
     return shape;
 }
 
-ShapePtr make_spline(const std::list<Eigen::Vector3d> &points,
-                     const Eigen::Vector3d &color,
-                     const double &opacity) {
+ShapePtr make_spline(const std::list<Eigen::Vector3d>& points, const Eigen::Vector3d& color,
+                     const double& opacity) {
     auto shape = make_shape(color, opacity);
     for (auto point : points) {
         auto p = shape->mutable_spline()->add_point();
@@ -257,10 +219,8 @@ ShapePtr make_spline(const std::list<Eigen::Vector3d> &points,
     return shape;
 }
 
-ShapePtr make_text(const std::string &text,
-                   const Eigen::Vector3d &center,
-                   const Eigen::Vector3d &color,
-                   const double &opacity) {
+ShapePtr make_text(const std::string& text, const Eigen::Vector3d& center,
+                   const Eigen::Vector3d& color, const double& opacity) {
     auto shape = make_shape(color, opacity);
     shape->mutable_text()->set_text(text);
     shape->mutable_text()->set_scale(1.0);
@@ -268,11 +228,9 @@ ShapePtr make_text(const std::string &text,
     return shape;
 }
 
-ShapePtr make_triangle(const Eigen::Vector3d &point0,
-                       const Eigen::Vector3d &point1,
-                       const Eigen::Vector3d &point2,
-                       const Eigen::Vector3d &color,
-                       const double &opacity) {
+ShapePtr make_triangle(const Eigen::Vector3d& point0, const Eigen::Vector3d& point1,
+                       const Eigen::Vector3d& point2, const Eigen::Vector3d& color,
+                       const double& opacity) {
     auto shape = make_shape(color, opacity);
     sc::set(shape->mutable_triangle()->mutable_point0(), point0);
     sc::set(shape->mutable_triangle()->mutable_point1(), point1);
@@ -280,5 +238,5 @@ ShapePtr make_triangle(const Eigen::Vector3d &point0,
     return shape;
 }
 
-} // namespace shape
-} // namespace scrimmage
+}  // namespace shape
+}  // namespace scrimmage
diff --git a/src/common/Timer.cpp b/src/common/Timer.cpp
index 1ebc99ba9d..1c7a8b39c0 100644
--- a/src/common/Timer.cpp
+++ b/src/common/Timer.cpp
@@ -32,9 +32,8 @@
 
 #include <scrimmage/common/Timer.h>
 
-#include <ctime>
-
 #include <boost/thread.hpp>
+#include <ctime>
 
 namespace scrimmage {
 
@@ -104,7 +103,7 @@ uint64_t Timer::getnanotime() {
     uint64_t nano = 0;
     timespec ts;
     // clock_gettime(CLOCK_MONOTONIC, &ts); // Works on FreeBSD
-    clock_gettime(CLOCK_REALTIME, &ts); // Works on Linux
+    clock_gettime(CLOCK_REALTIME, &ts);  // Works on Linux
 
     nano = ts.tv_sec * 1e9 + ts.tv_nsec;
     return nano;
@@ -136,4 +135,4 @@ double Timer::time_warp() {
     return time_warp_;
 }
 
-} // namespace scrimmage
+}  // namespace scrimmage
diff --git a/src/common/Utilities.cpp b/src/common/Utilities.cpp
index 450fa98662..e5ed859c9c 100644
--- a/src/common/Utilities.cpp
+++ b/src/common/Utilities.cpp
@@ -32,15 +32,14 @@
 
 #include <scrimmage/common/Utilities.h>
 
-#include <iostream>
+#include <boost/algorithm/string.hpp>
+#include <cassert>
 #include <iomanip>
+#include <iostream>
+#include <memory>
 #include <sstream>
 #include <string>
-#include <cassert>
 #include <vector>
-#include <memory>
-
-#include <boost/algorithm/string.hpp>
 
 #define BOOST_NO_CXX11_SCOPED_ENUMS
 #include <boost/filesystem.hpp>
@@ -49,9 +48,8 @@ namespace fs = boost::filesystem;
 
 namespace scrimmage {
 
-int next_available_id(std::string name,
-                      std::map<std::string, std::string> &info,
-                      std::map<int, int> &id_map) {
+int next_available_id(std::string name, std::map<std::string, std::string>& info,
+                      std::map<int, int>& id_map) {
     int id;
     if (info.count(name) > 0) {
         id = std::stoi(info[name]);
@@ -60,7 +58,8 @@ int next_available_id(std::string name,
     }
 
     // Find the next available ID:
-    while (id_map.count(id) > 0) id++;
+    while (id_map.count(id) > 0)
+        id++;
 
     // Save the id in the map:
     id_map[id] = id;
@@ -85,18 +84,16 @@ void display_progress(float progress) {
     std::cout.flush();
 }
 
-std::string get_sha(std::string &path) {
+std::string get_sha(std::string& path) {
     std::string cd_cmd = "cd " + path + " && ";
     std::string sha_cmd = cd_cmd + "git rev-parse HEAD | tr -d '\n'";
     std::string status_cmd = cd_cmd + "git status --porcelain | wc -l";
 
-    FILE *sha_file = popen(sha_cmd.c_str(), "r");
-    FILE *status_file = popen(status_cmd.c_str(), "r");
+    FILE* sha_file = popen(sha_cmd.c_str(), "r");
+    FILE* status_file = popen(status_cmd.c_str(), "r");
 
     char sha[41], status[3];
-    bool success =
-        fgets(sha, 40, sha_file) != NULL &&
-        fgets(status, 2, status_file) != NULL;
+    bool success = fgets(sha, 40, sha_file) != NULL && fgets(status, 2, status_file) != NULL;
 
     pclose(sha_file);
     pclose(status_file);
@@ -111,7 +108,7 @@ std::string get_sha(std::string &path) {
 }
 
 std::string get_version() {
-    const char *env = std::getenv("SCRIMMAGE_ROOT");
+    const char* env = std::getenv("SCRIMMAGE_ROOT");
     if (env == NULL) {
         return std::string("");
     } else {
@@ -120,41 +117,38 @@ std::string get_version() {
     }
 }
 
-void filter_line(int downsampling_factor,
-    int num_points,
-    std::vector<Eigen::Vector3d> &path,
-    std::vector<Eigen::Vector3d> &filtered_path) {
-
+void filter_line(int downsampling_factor, int num_points, std::vector<Eigen::Vector3d>& path,
+                 std::vector<Eigen::Vector3d>& filtered_path) {
     int curvature_sz = path.size() / downsampling_factor;
     std::list<std::pair<int, double>> curvature;
 
-    auto idx = [=](int i) {return downsampling_factor * i;};
+    auto idx = [=](int i) { return downsampling_factor * i; };
     for (int i = 1; i < curvature_sz - 1; i++) {
-        Eigen::Vector3d &pt_prev = path[idx(i - 1)];
-        Eigen::Vector3d &pt = path[idx(i)];
-        Eigen::Vector3d &pt_next = path[idx(i + 1)];
+        Eigen::Vector3d& pt_prev = path[idx(i - 1)];
+        Eigen::Vector3d& pt = path[idx(i)];
+        Eigen::Vector3d& pt_next = path[idx(i + 1)];
 
         double curv = (pt_next - 2 * pt + pt_prev).squaredNorm();
         curvature.push_back(std::make_pair(idx(i), curv));
     }
 
     using Pair = std::pair<int, double>;
-    curvature.sort([](Pair &a, Pair &b) {return a.second > b.second;});
+    curvature.sort([](Pair& a, Pair& b) { return a.second > b.second; });
     curvature.erase(std::next(curvature.begin(), num_points), curvature.end());
-    curvature.sort([](Pair &a, Pair &b) {return a.first < b.first;});
+    curvature.sort([](Pair& a, Pair& b) { return a.first < b.first; });
 
     filtered_path.clear();
     filtered_path.reserve(curvature.size() + 2);
     filtered_path.push_back(path[0]);
 
-    for (Pair &p : curvature) {
+    for (Pair& p : curvature) {
         filtered_path.push_back(path[p.first]);
     }
 
     filtered_path.push_back(path.back());
 }
 
-std::string generate_chars(const std::string &symbol, int num) {
+std::string generate_chars(const std::string& symbol, int num) {
     std::string out = "";
     for (int i = 0; i < num; i++) {
         out += symbol;
@@ -162,7 +156,7 @@ std::string generate_chars(const std::string &symbol, int num) {
     return out;
 }
 
-std::string eigen_str(const Eigen::VectorXd &vec, uint8_t precision) {
+std::string eigen_str(const Eigen::VectorXd& vec, uint8_t precision) {
     if (vec.size() == 0) {
         return "";
     } else {
@@ -191,4 +185,4 @@ std::vector<double> linspace(double low, double high, uint32_t n) {
     return out;
 }
 
-} // namespace scrimmage
+}  // namespace scrimmage
diff --git a/src/common/VariableIO.cpp b/src/common/VariableIO.cpp
index 92e221c151..2d5fb34dff 100644
--- a/src/common/VariableIO.cpp
+++ b/src/common/VariableIO.cpp
@@ -33,22 +33,20 @@
 #include <scrimmage/common/VariableIO.h>
 
 #include <Eigen/Dense>
-
+#include <boost/range/adaptor/map.hpp>
+#include <boost/range/algorithm/set_algorithm.hpp>
+#include <iomanip>
 #include <iostream>
 #include <map>
 #include <string>
-#include <iomanip>
 #include <vector>
 
-#include <boost/range/algorithm/set_algorithm.hpp>
-#include <boost/range/adaptor/map.hpp>
-
 namespace br = boost::range;
 namespace ba = boost::adaptors;
 
 namespace scrimmage {
 
-std::map<VariableIO::Type, std::string>VariableIO::type_map_ = {
+std::map<VariableIO::Type, std::string> VariableIO::type_map_ = {
     {VariableIO::Type::desired_altitude, "desired_altitude"},
     {VariableIO::Type::desired_speed, "desired_speed"},
     {VariableIO::Type::desired_heading, "desired_heading"},
@@ -74,22 +72,21 @@ std::map<VariableIO::Type, std::string>VariableIO::type_map_ = {
     {VariableIO::Type::position_z, "position_z"},
     {VariableIO::Type::acceleration_x, "acceleration_x"},
     {VariableIO::Type::acceleration_y, "acceleration_y"},
-    {VariableIO::Type::acceleration_z, "acceleration_z"}
-};
+    {VariableIO::Type::acceleration_z, "acceleration_z"}};
 
-VariableIO::VariableIO() : input_(std::make_shared<Eigen::VectorXd>()),
-                           output_(std::make_shared<Eigen::VectorXd>()) {
+VariableIO::VariableIO()
+    : input_(std::make_shared<Eigen::VectorXd>()), output_(std::make_shared<Eigen::VectorXd>()) {
 }
 
-std::map<std::string, int> & VariableIO::output_variable_index() {
+std::map<std::string, int>& VariableIO::output_variable_index() {
     return output_variable_index_;
 }
 
-std::map<std::string, int> & VariableIO::input_variable_index() {
+std::map<std::string, int>& VariableIO::input_variable_index() {
     return input_variable_index_;
 }
 
-int VariableIO::add_input_variable(const std::string &var) {
+int VariableIO::add_input_variable(const std::string& var) {
     // If the variable already exists, return its existing index
     declared_input_variables_.insert(var);
     auto it = input_variable_index_.find(var);
@@ -105,7 +102,7 @@ int VariableIO::add_input_variable(const std::string &var) {
     return idx;
 }
 
-int VariableIO::add_output_variable(const std::string &var) {
+int VariableIO::add_output_variable(const std::string& var) {
     // If the variable already exists, return its existing index
     declared_output_variables_.insert(var);
     int idx;
@@ -155,7 +152,7 @@ double VariableIO::output(int i) {
     return i < output_->size() ? (*output_)(i) : NAN;
 }
 
-void connect(VariableIO &output, VariableIO &input) {
+void connect(VariableIO& output, VariableIO& input) {
     output.output_variable_index() = input.input_variable_index();
     output.output_ = input.input_;
 }
@@ -184,46 +181,43 @@ std::set<std::string> VariableIO::declared_output_variables() {
     return declared_output_variables_;
 }
 
-void VariableIO::set_input(const std::shared_ptr<Eigen::VectorXd> &input) {
+void VariableIO::set_input(const std::shared_ptr<Eigen::VectorXd>& input) {
     input_ = input;
 }
-void VariableIO::set_output(const std::shared_ptr<Eigen::VectorXd> &output) {
+void VariableIO::set_output(const std::shared_ptr<Eigen::VectorXd>& output) {
     output_ = output;
 }
 
-bool verify_io_connection(VariableIO &output, VariableIO &input) {
+bool verify_io_connection(VariableIO& output, VariableIO& input) {
     std::vector<std::string> mismatched_keys;
-    br::set_difference(
-        input.declared_input_variables(),
-        output.declared_output_variables(),
-        std::back_inserter(mismatched_keys));
+    br::set_difference(input.declared_input_variables(), output.declared_output_variables(),
+                       std::back_inserter(mismatched_keys));
 
     return mismatched_keys.empty();
 }
 
-void print_io_error(const std::string &in_name, VariableIO &v) {
+void print_io_error(const std::string& in_name, VariableIO& v) {
     auto keys = v.input_variable_index() | ba::map_keys;
 
     std::cout << "First, include the VariableIO class in the cpp file: "
               << "#include <scrimmage/common/VariableIO.h>" << std::endl;
 
     std::cout << "Second, place the following in its initializer: " << std::endl;
-    for (const std::string &key : keys) {
-        std::cout << "    " << key << "_idx_ = vars_.declare("
-                  << std::quoted(key) << ", scrimmage::VariableIO::Direction::Out);"
-                  << std::endl;
+    for (const std::string& key : keys) {
+        std::cout << "    " << key << "_idx_ = vars_.declare(" << std::quoted(key)
+                  << ", scrimmage::VariableIO::Direction::Out);" << std::endl;
     }
 
     std::cout << "Third, place the following in its step function: " << std::endl;
-    for (const std::string &key : keys) {
+    for (const std::string& key : keys) {
         std::cout << "    vars_.output(" << key << "_idx_, value_to_output);" << std::endl;
     }
-    std::cout << "where value_to_output is what you want " << in_name
-              << " to receive as its input." << std::endl;
+    std::cout << "where value_to_output is what you want " << in_name << " to receive as its input."
+              << std::endl;
 
     std::cout << "Third, place following in the class declaration: " << std::endl;
-    for (const std::string &key : keys) {
+    for (const std::string& key : keys) {
         std::cout << "    uint8_t " << key << "_idx_ = 0;" << std::endl;
     }
 }
-} // namespace scrimmage
+}  // namespace scrimmage
diff --git a/src/common/Waypoint.cpp b/src/common/Waypoint.cpp
index aa7cdef363..c0b2202125 100644
--- a/src/common/Waypoint.cpp
+++ b/src/common/Waypoint.cpp
@@ -33,11 +33,10 @@
 #include <scrimmage/math/State.h>
 #include <scrimmage/proto/ProtoConversions.h>
 
+#include <GeographicLib/LocalCartesian.hpp>
 #include <iomanip>
-#include <limits>
 #include <iostream>
-
-#include <GeographicLib/LocalCartesian.hpp>
+#include <limits>
 
 namespace sc = scrimmage;
 namespace sc_msgs = scrimmage_msgs;
@@ -45,28 +44,29 @@ namespace sc_msgs = scrimmage_msgs;
 namespace scrimmage {
 namespace autonomy {
 
-Waypoint::Waypoint(const double& latitude, const double& longitude,
-                   const double& altitude)
-    : latitude_(latitude), longitude_(longitude), altitude_(altitude) {}
+Waypoint::Waypoint(const double& latitude, const double& longitude, const double& altitude)
+    : latitude_(latitude), longitude_(longitude), altitude_(altitude) {
+}
 
-Waypoint::Waypoint(const double &x, const double& y, const double& z,
-                   const std::shared_ptr<GeographicLib::LocalCartesian> &proj) {
+Waypoint::Waypoint(const double& x, const double& y, const double& z,
+                   const std::shared_ptr<GeographicLib::LocalCartesian>& proj) {
     proj->Reverse(x, y, z, latitude_, longitude_, altitude_);
 }
 
-Waypoint::Waypoint(const Eigen::Vector3d &xyz,
-                   const std::shared_ptr<GeographicLib::LocalCartesian> &proj) {
+Waypoint::Waypoint(const Eigen::Vector3d& xyz,
+                   const std::shared_ptr<GeographicLib::LocalCartesian>& proj) {
     proj->Reverse(xyz(0), xyz(1), xyz(2), latitude_, longitude_, altitude_);
 }
 
-Waypoint::Waypoint(const scrimmage_msgs::Waypoint &wp)
-        : id_(wp.id()), time_(wp.time()),
-          position_tolerance_(wp.position_tolerance()),
-          quat_tolerance_(wp.quat_tolerance()),
-          tolerance_in_2d_(wp.tolerance_in_2d()) {
+Waypoint::Waypoint(const scrimmage_msgs::Waypoint& wp)
+    : id_(wp.id()),
+      time_(wp.time()),
+      position_tolerance_(wp.position_tolerance()),
+      quat_tolerance_(wp.quat_tolerance()),
+      tolerance_in_2d_(wp.tolerance_in_2d()) {
     sc::set(quat_, wp.quat());
 
-    auto get_alt = [] (const sc_msgs::PositionLLA &lla) {
+    auto get_alt = [](const sc_msgs::PositionLLA& lla) {
         double alt = 0;
         if (lla.alt_oneof_case() == sc_msgs::PositionLLA::kAltMsl) {
             alt = lla.alt_msl();
@@ -85,8 +85,8 @@ Waypoint::Waypoint(const scrimmage_msgs::Waypoint &wp)
         GeographicLib::LocalCartesian proj(wp.xyz().origin().latitude(),
                                            wp.xyz().origin().longitude(),
                                            get_alt(wp.xyz().origin()));
-        proj.Reverse(wp.xyz().xyz().x(), wp.xyz().xyz().y(), wp.xyz().xyz().z(),
-                     latitude_, longitude_, altitude_);
+        proj.Reverse(wp.xyz().xyz().x(), wp.xyz().xyz().y(), wp.xyz().xyz().z(), latitude_,
+                     longitude_, altitude_);
     }
 }
 
@@ -104,15 +104,12 @@ scrimmage_msgs::Waypoint Waypoint::lla_proto() {
     return wp;
 }
 
-bool Waypoint::is_within_tolerance(
-    const scrimmage::StatePtr &state,
-    const std::shared_ptr<GeographicLib::LocalCartesian> &proj) {
-
+bool Waypoint::is_within_tolerance(const scrimmage::StatePtr& state,
+                                   const std::shared_ptr<GeographicLib::LocalCartesian>& proj) {
     Eigen::Vector3d pos;
     proj->Forward(latitude_, longitude_, altitude_, pos(0), pos(1), pos(2));
-    double dist = tolerance_in_2d_ ?
-            (state->pos().head<2>() - pos.head<2>()).norm() :
-            (state->pos() - pos).norm();
+    double dist = tolerance_in_2d_ ? (state->pos().head<2>() - pos.head<2>()).norm()
+                                   : (state->pos() - pos).norm();
     return dist <= position_tolerance_;
 }
 
@@ -125,29 +122,24 @@ void Waypoint::set_quat_tolerance(const double& quat_tol) {
 }
 
 std::ostream& operator<<(std::ostream& os, Waypoint& wp) {
-    os << std::setprecision(10)
-       << wp.time() << ", "
-       << wp.latitude() << ", " << wp.longitude() << ", " << wp.altitude()
-       << wp.quat() << ", "
-       << wp.position_tolerance() << ", "
+    os << std::setprecision(10) << wp.time() << ", " << wp.latitude() << ", " << wp.longitude()
+       << ", " << wp.altitude() << wp.quat() << ", " << wp.position_tolerance() << ", "
        << wp.quat_tolerance();
     return os;
 }
 
-bool operator==(const Waypoint &lhs, const Waypoint &rhs) {
-    auto far = [&](double a, double b) {return std::abs(a - b) > 1.0e-6;};
+bool operator==(const Waypoint& lhs, const Waypoint& rhs) {
+    auto far = [&](double a, double b) { return std::abs(a - b) > 1.0e-6; };
 
-    return !far(lhs.altitude(), rhs.latitude()) &&
-           !far(lhs.longitude(), rhs.longitude()) &&
-           !far(lhs.altitude(), rhs.altitude());
+    return !far(lhs.altitude(), rhs.latitude()) && !far(lhs.longitude(), rhs.longitude())
+           && !far(lhs.altitude(), rhs.altitude());
 }
 
-Eigen::Vector3d Waypoint::xyz(
-    const std::shared_ptr<GeographicLib::LocalCartesian> &proj) const {
+Eigen::Vector3d Waypoint::xyz(const std::shared_ptr<GeographicLib::LocalCartesian>& proj) const {
     Eigen::Vector3d p;
     proj->Forward(latitude_, longitude_, altitude_, p(0), p(1), p(2));
     return p;
 }
 
-} // namespace autonomy
-} // namespace scrimmage
+}  // namespace autonomy
+}  // namespace scrimmage
diff --git a/src/common/WaypointListProcessor.cpp b/src/common/WaypointListProcessor.cpp
index 55b60a0acc..91e72a465e 100644
--- a/src/common/WaypointListProcessor.cpp
+++ b/src/common/WaypointListProcessor.cpp
@@ -40,8 +40,7 @@ namespace sp = scrimmage_proto;
 namespace scrimmage {
 namespace autonomy {
 
-void WaypointListProcessor::set_waypoint_list(
-    const scrimmage_msgs::WaypointList &wp_list) {
+void WaypointListProcessor::set_waypoint_list(const scrimmage_msgs::WaypointList& wp_list) {
     returning_stage_ = false;
 
     // Construct a list of Waypoints from protobuf waypoint list
@@ -52,13 +51,13 @@ void WaypointListProcessor::set_waypoint_list(
 
     // Filter the waypoint list to ensure that two waypoints in succession
     // are not equal. Some planners generate duplicate waypoints.
-    auto wps_are_close = [] (auto &wp0, auto &wp1) {
-        auto numbers_are_close = [] (const double &a, const double &b) {
+    auto wps_are_close = [](auto& wp0, auto& wp1) {
+        auto numbers_are_close = [](const double& a, const double& b) {
             return std::abs(a - b) < std::numeric_limits<double>::epsilon();
         };
         return numbers_are_close(wp0.latitude(), wp1.latitude())
-                && numbers_are_close(wp0.longitude(), wp1.longitude())
-                && numbers_are_close(wp0.altitude(), wp1.altitude());
+               && numbers_are_close(wp0.longitude(), wp1.longitude())
+               && numbers_are_close(wp0.altitude(), wp1.altitude());
     };
     wp_list_.unique(wps_are_close);
 
@@ -68,15 +67,13 @@ void WaypointListProcessor::set_waypoint_list(
     next_wp_it_ = next_waypoint(curr_wp_it_);
 }
 
-WaypointListProcessor::Status
-WaypointListProcessor::process(
-    const scrimmage::StatePtr &state,
-    const std::shared_ptr<GeographicLib::LocalCartesian> &proj) {
+WaypointListProcessor::Status WaypointListProcessor::process(
+    const scrimmage::StatePtr& state, const std::shared_ptr<GeographicLib::LocalCartesian>& proj) {
     if (wp_list_.size() == 0) {
         return Status::Empty;
     }
 
-    Waypoint &curr_wp_lla = *curr_wp_it_;
+    Waypoint& curr_wp_lla = *curr_wp_it_;
 
     // If the current state isn't within tolerance, the waypoint is unchanged
     if (not curr_wp_lla.is_within_tolerance(state, proj)) {
@@ -90,8 +87,8 @@ WaypointListProcessor::process(
     return Status::Changed;
 }
 
-std::list<Waypoint>::iterator WaypointListProcessor::
-next_waypoint(const std::list<Waypoint>::iterator &it_wp) {
+std::list<Waypoint>::iterator WaypointListProcessor::next_waypoint(
+    const std::list<Waypoint>::iterator& it_wp) {
     std::list<Waypoint>::iterator it_next;
     switch (mode_) {
         case scrimmage_msgs::WaypointList::FOLLOW_ONCE:
@@ -99,7 +96,7 @@ next_waypoint(const std::list<Waypoint>::iterator &it_wp) {
             break;
 
         case scrimmage_msgs::WaypointList::BACK_AND_FORTH:
-            it_next = returning_stage_ ? std::prev(it_wp): std::next(it_wp);
+            it_next = returning_stage_ ? std::prev(it_wp) : std::next(it_wp);
 
             if (it_next == wp_list_.begin()) {
                 returning_stage_ = false;
@@ -122,32 +119,26 @@ next_waypoint(const std::list<Waypoint>::iterator &it_wp) {
             }
             break;
 
-        default :
+        default:
             std::string msg = "Waypoint mode not defined yet.";
             throw std::runtime_error(msg);
     }
     return it_next;
 }
 
-boost::optional<const Waypoint&>
-WaypointListProcessor::previous_waypoint() {
-    return prev_wp_it_ != wp_list_.end() ?
-            boost::optional<const Waypoint&>{*prev_wp_it_} :
-            boost::optional<const Waypoint&>{};
+boost::optional<const Waypoint&> WaypointListProcessor::previous_waypoint() {
+    return prev_wp_it_ != wp_list_.end() ? boost::optional<const Waypoint&>{*prev_wp_it_}
+                                         : boost::optional<const Waypoint&>{};
 }
 
-boost::optional<const Waypoint&>
-WaypointListProcessor::current_waypoint() {
-    return curr_wp_it_ != wp_list_.end() ?
-            boost::optional<const Waypoint&>{*curr_wp_it_} :
-            boost::optional<const Waypoint&>{};
+boost::optional<const Waypoint&> WaypointListProcessor::current_waypoint() {
+    return curr_wp_it_ != wp_list_.end() ? boost::optional<const Waypoint&>{*curr_wp_it_}
+                                         : boost::optional<const Waypoint&>{};
 }
 
-boost::optional<const Waypoint&>
-WaypointListProcessor::next_waypoint() {
-    return next_wp_it_ != wp_list_.end() ?
-            boost::optional<const Waypoint&>{*next_wp_it_} :
-            boost::optional<const Waypoint&>{};
+boost::optional<const Waypoint&> WaypointListProcessor::next_waypoint() {
+    return next_wp_it_ != wp_list_.end() ? boost::optional<const Waypoint&>{*next_wp_it_}
+                                         : boost::optional<const Waypoint&>{};
 }
-} // namespace autonomy
-} // namespace scrimmage
+}  // namespace autonomy
+}  // namespace scrimmage
diff --git a/src/entity/Contact.cpp b/src/entity/Contact.cpp
index 8011d9f450..b7bd286ff7 100644
--- a/src/entity/Contact.cpp
+++ b/src/entity/Contact.cpp
@@ -30,52 +30,79 @@
  *
  */
 
+#include <scrimmage/common/ID.h>
 #include <scrimmage/entity/Contact.h>
 #include <scrimmage/math/State.h>
-#include <scrimmage/common/ID.h>
 
-#include <memory>
 #include <iostream>
+#include <memory>
 
 namespace scrimmage {
 
-Contact::Contact() : state_(std::make_shared<State>()) {}
+Contact::Contact() : state_(std::make_shared<State>()) {
+}
 
-Contact::Contact(const ID &id, const StatePtr &state) :
-    id_(id), state_(state) {
+Contact::Contact(const ID& id, const StatePtr& state) : id_(id), state_(state) {
 }
 
-Contact::Contact(ID &id, double radius, StatePtr &state, Type type,
+Contact::Contact(ID& id, double radius, StatePtr& state, Type type,
                  scrimmage_proto::ContactVisualPtr cv,
-                 const std::unordered_map<std::string, MessageBasePtr> &properties) :
-    id_(id), state_(state), type_(type), contact_visual_(cv),
-    active_(true), radius_(radius), properties_(properties) {}
+                 const std::unordered_map<std::string, MessageBasePtr>& properties)
+    : id_(id),
+      state_(state),
+      type_(type),
+      contact_visual_(cv),
+      active_(true),
+      radius_(radius),
+      properties_(properties) {
+}
 
-void Contact::set_id(const ID &id) { id_ = id; }
+void Contact::set_id(const ID& id) {
+    id_ = id;
+}
 
-ID &Contact::id() { return id_; }
+ID& Contact::id() {
+    return id_;
+}
 
-void Contact::set_state(StatePtr &state) { state_ = state; }
+void Contact::set_state(StatePtr& state) {
+    state_ = state;
+}
 
-StatePtr &Contact::state() { return state_; }
+StatePtr& Contact::state() {
+    return state_;
+}
 
-std::shared_ptr<const State> Contact::state_const() const { return state_; }
+std::shared_ptr<const State> Contact::state_const() const {
+    return state_;
+}
 
-void Contact::set_type(Contact::Type type) { type_ = type; }
+void Contact::set_type(Contact::Type type) {
+    type_ = type;
+}
 
-Contact::Type Contact::type() { return type_; }
+Contact::Type Contact::type() {
+    return type_;
+}
 
-scrimmage_proto::ContactVisualPtr &Contact::contact_visual()
-{ return contact_visual_; }
+scrimmage_proto::ContactVisualPtr& Contact::contact_visual() {
+    return contact_visual_;
+}
 
-void Contact::set_active(bool active) { active_ = active; }
+void Contact::set_active(bool active) {
+    active_ = active;
+}
 
-void Contact::set_radius(double radius) { radius_ = radius; }
+void Contact::set_radius(double radius) {
+    radius_ = radius;
+}
 
-bool Contact::active() { return active_; }
+bool Contact::active() {
+    return active_;
+}
 
 std::ostream& operator<<(std::ostream& os, const Contact& c) {
     os << c.id_ << ": " << *c.state_;
     return os;
 }
-} // namespace scrimmage
+}  // namespace scrimmage
diff --git a/src/entity/Entity.cpp b/src/entity/Entity.cpp
index 1ae63f00cf..89c9c894ed 100644
--- a/src/entity/Entity.cpp
+++ b/src/entity/Entity.cpp
@@ -31,32 +31,31 @@
  */
 
 #include <scrimmage/autonomy/Autonomy.h>
-#include <scrimmage/common/Utilities.h>
 #include <scrimmage/common/GlobalService.h>
+#include <scrimmage/common/Utilities.h>
 #include <scrimmage/entity/Entity.h>
-#include <scrimmage/math/State.h>
+#include <scrimmage/entity/EntityPluginHelper.h>
 #include <scrimmage/math/Angles.h>
-#include <scrimmage/motion/MotionModel.h>
+#include <scrimmage/math/State.h>
 #include <scrimmage/motion/Controller.h>
-#include <scrimmage/parse/MissionParse.h>
+#include <scrimmage/motion/MotionModel.h>
 #include <scrimmage/parse/ConfigParse.h>
+#include <scrimmage/parse/MissionParse.h>
 #include <scrimmage/parse/ParseUtils.h>
 #include <scrimmage/plugin_manager/PluginManager.h>
 #include <scrimmage/proto/ProtoConversions.h>
 #include <scrimmage/sensor/Sensor.h>
 #include <scrimmage/simcontrol/SimUtils.h>
-#include <scrimmage/entity/EntityPluginHelper.h>
 
-#include <iostream>
-#include <iomanip>
-#include <memory>
 #include <algorithm>
-
 #include <boost/algorithm/cxx11/none_of.hpp>
 #include <boost/algorithm/string/case_conv.hpp>
-#include <boost/range/algorithm/copy.hpp>
-#include <boost/range/adaptor/transformed.hpp>
 #include <boost/lexical_cast.hpp>
+#include <boost/range/adaptor/transformed.hpp>
+#include <boost/range/algorithm/copy.hpp>
+#include <iomanip>
+#include <iostream>
+#include <memory>
 
 using std::cout;
 using std::endl;
@@ -67,23 +66,15 @@ namespace ba = boost::adaptors;
 
 namespace scrimmage {
 
-bool Entity::init(AttributeMap &overrides,
-                  std::map<std::string, std::string> &info,
-                  std::shared_ptr<std::unordered_map<int, int>> &id_to_team_map,
-                  std::shared_ptr<std::unordered_map<int, EntityPtr>> &id_to_ent_map,
-                  ContactMapPtr &contacts,
-                  MissionParsePtr mp,
-                  const std::shared_ptr<GeographicLib::LocalCartesian> &proj,
-                  int id, int ent_desc_id,
-                  PluginManagerPtr plugin_manager,
-                  FileSearchPtr &file_search,
-                  RTreePtr &rtree,
-                  PubSubPtr &pubsub,
-                  PrintPtr &printer,
-                  TimePtr &time,
-                  const ParameterServerPtr &param_server,
-                  const GlobalServicePtr &global_services,
-                  const std::set<std::string> &plugin_tags,
+bool Entity::init(AttributeMap& overrides, std::map<std::string, std::string>& info,
+                  std::shared_ptr<std::unordered_map<int, int>>& id_to_team_map,
+                  std::shared_ptr<std::unordered_map<int, EntityPtr>>& id_to_ent_map,
+                  ContactMapPtr& contacts, MissionParsePtr mp,
+                  const std::shared_ptr<GeographicLib::LocalCartesian>& proj, int id,
+                  int ent_desc_id, PluginManagerPtr plugin_manager, FileSearchPtr& file_search,
+                  RTreePtr& rtree, PubSubPtr& pubsub, PrintPtr& printer, TimePtr& time,
+                  const ParameterServerPtr& param_server, const GlobalServicePtr& global_services,
+                  const std::set<std::string>& plugin_tags,
                   std::function<void(std::map<std::string, std::string>&)> param_override_func,
                   const int& debug_level) {
     pubsub_ = pubsub;
@@ -134,9 +125,9 @@ bool Entity::init(AttributeMap &overrides,
 
     double sp = get("speed", info, 0.0);
     if (sp > 0 && vx == 0 && vy == 0 && vz == 0) {
-      Eigen::Vector3d relative_vel_vector = Eigen::Vector3d::UnitX()*sp;
-      Eigen::Vector3d vel_vector = state_->quat().rotate(relative_vel_vector);
-      state_->vel() << vel_vector[0], vel_vector[1], vel_vector[2];
+        Eigen::Vector3d relative_vel_vector = Eigen::Vector3d::UnitX() * sp;
+        Eigen::Vector3d vel_vector = state_->quat().rotate(relative_vel_vector);
+        state_->vel() << vel_vector[0], vel_vector[1], vel_vector[2];
     }
 
     double roll = Angles::deg2rad(get("roll", info, 0.0));
@@ -156,21 +147,16 @@ bool Entity::init(AttributeMap &overrides,
     // The MissionParser appends the order number to the sensor (e.g., sensor0,
     // sensor1, etc.)
     int sensor_ct = 0;
-    std::string sensor_order_name = std::string("sensor") +
-        std::to_string(sensor_ct);
+    std::string sensor_order_name = std::string("sensor") + std::to_string(sensor_ct);
 
     while (info.count(sensor_order_name) > 0) {
         ConfigParse config_parse;
         std::string sensor_name = info[sensor_order_name];
-        PluginStatus<Sensor> status =
-            plugin_manager->make_plugin<Sensor>("scrimmage::Sensor",
-                                                sensor_name, *file_search,
-                                                config_parse,
-                                                overrides[sensor_order_name],
-                                                plugin_tags);
+        PluginStatus<Sensor> status = plugin_manager->make_plugin<Sensor>(
+            "scrimmage::Sensor", sensor_name, *file_search, config_parse,
+            overrides[sensor_order_name], plugin_tags);
         if (status.status == PluginStatus<Sensor>::cast_failed) {
-            std::cout << "Failed to open sensor plugin: " << sensor_name
-                      << std::endl;
+            std::cout << "Failed to open sensor plugin: " << sensor_name << std::endl;
             return false;
         } else if (status.status == PluginStatus<Sensor>::parse_failed) {
             return false;
@@ -191,8 +177,7 @@ bool Entity::init(AttributeMap &overrides,
             if (it_rpy != overrides[sensor_order_name].end()) {
                 str2container(it_rpy->second, " ", tf_rpy, 3);
             }
-            sensor->transform()->quat().set(Angles::deg2rad(tf_rpy[0]),
-                                            Angles::deg2rad(tf_rpy[1]),
+            sensor->transform()->quat().set(Angles::deg2rad(tf_rpy[0]), Angles::deg2rad(tf_rpy[1]),
                                             Angles::deg2rad(tf_rpy[2]));
 
             sensor->set_parent(parent);
@@ -206,8 +191,8 @@ bool Entity::init(AttributeMap &overrides,
             // get loop rate from plugin's params
             auto it_loop_rate = config_parse.params().find("loop_rate");
             if (it_loop_rate != config_parse.params().end()) {
-              const double loop_rate = std::stod(it_loop_rate->second);
-              sensor->set_loop_rate(loop_rate);
+                const double loop_rate = std::stod(it_loop_rate->second);
+                sensor->set_loop_rate(loop_rate);
             }
 
             std::string given_name = sensor_name + std::to_string(sensor_ct);
@@ -230,13 +215,9 @@ bool Entity::init(AttributeMap &overrides,
     bool init_empty_motion_model = true;
     if (info.count("motion_model") > 0) {
         ConfigParse config_parse;
-        PluginStatus<MotionModel> status =
-            plugin_manager->make_plugin<MotionModel>("scrimmage::MotionModel",
-                                                     info["motion_model"],
-                                                     *file_search,
-                                                     config_parse,
-                                                     overrides["motion_model"],
-                                                     plugin_tags);
+        PluginStatus<MotionModel> status = plugin_manager->make_plugin<MotionModel>(
+            "scrimmage::MotionModel", info["motion_model"], *file_search, config_parse,
+            overrides["motion_model"], plugin_tags);
         if (status.status == PluginStatus<MotionModel>::cast_failed) {
             cout << "Failed to open motion model plugin: " << info["motion_model"] << endl;
             return false;
@@ -303,16 +284,11 @@ bool Entity::init(AttributeMap &overrides,
         std::string controller_name = *rit;
 
         ConfigParse config_parse;
-        PluginStatus<Controller> status =
-            plugin_manager_->make_plugin<Controller>("scrimmage::Controller",
-                                                     info[controller_name],
-                                                     *file_search,
-                                                     config_parse,
-                                                     overrides[controller_name],
-                                                     plugin_tags);
+        PluginStatus<Controller> status = plugin_manager_->make_plugin<Controller>(
+            "scrimmage::Controller", info[controller_name], *file_search, config_parse,
+            overrides[controller_name], plugin_tags);
         if (status.status == PluginStatus<Controller>::cast_failed) {
-            std::cout << "Failed to open controller plugin: "
-                      << controller_name << std::endl;
+            std::cout << "Failed to open controller plugin: " << controller_name << std::endl;
             return false;
         } else if (status.status == PluginStatus<Controller>::parse_failed) {
             return false;
@@ -332,8 +308,8 @@ bool Entity::init(AttributeMap &overrides,
             // get loop rate from plugin's params
             auto it_loop_rate = config_parse.params().find("loop_rate");
             if (it_loop_rate != config_parse.params().end()) {
-              const double loop_rate = std::stod(it_loop_rate->second);
-              controller->set_loop_rate(loop_rate);
+                const double loop_rate = std::stod(it_loop_rate->second);
+                controller->set_loop_rate(loop_rate);
             }
 
             // Connect this controller to the motion model if it is the last
@@ -358,21 +334,17 @@ bool Entity::init(AttributeMap &overrides,
             // Verify the VariableIO connection
             if (connect_to_motion_model) {
                 if (!verify_io_connection(controller->vars(), motion_model_->vars())) {
-                    std::cout << "VariableIO Error: "
-                              << std::quoted(controller->name())
+                    std::cout << "VariableIO Error: " << std::quoted(controller->name())
                               << " does not provide inputs required by motion model "
-                              << std::quoted(motion_model_->name())
-                              << ": ";
+                              << std::quoted(motion_model_->name()) << ": ";
                     print_io_error(motion_model_->name(), motion_model_->vars());
                     return false;
                 }
             } else if (not connect_to_motion_model) {
                 if (!verify_io_connection(controller->vars(), controllers_.back()->vars())) {
-                    std::cout << "VariableIO Error: "
-                              << std::quoted(controller->name())
+                    std::cout << "VariableIO Error: " << std::quoted(controller->name())
                               << " does not provide inputs required by next controller "
-                              << std::quoted(controllers_.back()->name())
-                              << ": ";
+                              << std::quoted(controllers_.back()->name()) << ": ";
                     print_io_error(controllers_.back()->name(), controllers_.back()->vars());
                     return false;
                 }
@@ -390,11 +362,10 @@ bool Entity::init(AttributeMap &overrides,
 
     // If the motion model requires any inputs and there are no controllers,
     // this is a VariableIO error.
-    if (motion_model_->vars().input_variable_index().size() > 0 &&
-        controllers_.size() == 0) {
+    if (motion_model_->vars().input_variable_index().size() > 0 && controllers_.size() == 0) {
         std::cout << "VariableIO Error: There are not any controllers that "
-                  << "provide the inputs required by "
-                  << std::quoted(motion_model_->name()) << std::endl;
+                  << "provide the inputs required by " << std::quoted(motion_model_->name())
+                  << std::endl;
         print_io_error(motion_model_->name(), motion_model_->vars());
         std::cout << "If you want to directly pass the outputs from the "
                   << "autonomy to the motion_model, see the DirectController "
@@ -417,11 +388,9 @@ bool Entity::init(AttributeMap &overrides,
     // Create the autonomy plugins from the autonomy_names list.
     for (auto autonomy_name : autonomy_names) {
         auto autonomy = make_autonomy<Autonomy>(
-            info[autonomy_name], plugin_manager, overrides[autonomy_name],
-            parent, state_, id_to_team_map, id_to_ent_map, proj_, contacts,
-            file_search, rtree, pubsub, time, param_server, plugin_tags,
-            param_override_func, controllers_,
-            debug_level);
+            info[autonomy_name], plugin_manager, overrides[autonomy_name], parent, state_,
+            id_to_team_map, id_to_ent_map, proj_, contacts, file_search, rtree, pubsub, time,
+            param_server, plugin_tags, param_override_func, controllers_, debug_level);
 
         if (autonomy) {
             autonomies_.push_back(*autonomy);
@@ -435,18 +404,18 @@ bool Entity::init(AttributeMap &overrides,
 
     // Verify that at least one autonomy provides the inputs to the first
     // controller if the first controller requires some VariableIO input.
-    if (connect_entity && not controllers_.empty() &&
-        controllers_.front()->vars().input_variable_index().size() > 0) {
-        auto verify_io = [&](auto &autonomy) {
-            return verify_io_connection(autonomy->vars(),
-                                        controllers_.front()->vars());};
+    if (connect_entity && not controllers_.empty()
+        && controllers_.front()->vars().input_variable_index().size() > 0) {
+        auto verify_io = [&](auto& autonomy) {
+            return verify_io_connection(autonomy->vars(), controllers_.front()->vars());
+        };
         if (boost::algorithm::none_of(autonomies_, verify_io)) {
             auto out_it = std::ostream_iterator<std::string>(std::cout, ", ");
             std::cout << "VariableIO Error: "
                       << "no autonomies provide inputs required by Controller "
                       << std::quoted(controllers_.front()->name())
                       << ". Add VariableIO output declarations in ";
-            auto get_name = [&](auto &p) {return p->name();};
+            auto get_name = [&](auto& p) { return p->name(); };
             br::copy(autonomies_ | ba::transformed(get_name), out_it);
             std::cout << "as follows " << std::endl;
 
@@ -465,9 +434,8 @@ bool Entity::init(AttributeMap &overrides,
     return true;
 }
 
-bool Entity::parse_visual(std::map<std::string, std::string> &info,
-                          MissionParsePtr mp,
-                          std::map<std::string, std::string> &overrides) {
+bool Entity::parse_visual(std::map<std::string, std::string>& info, MissionParsePtr mp,
+                          std::map<std::string, std::string>& overrides) {
     visual_->set_id(id_.id());
     visual_->set_opacity(1.0);
 
@@ -478,15 +446,13 @@ bool Entity::parse_visual(std::map<std::string, std::string> &info,
         return true;
     }
 
-    find_model_properties(it->second, cv_parse,
-                          *file_search_, overrides, visual_,
-                          mesh_found, texture_found);
+    find_model_properties(it->second, cv_parse, *file_search_, overrides, visual_, mesh_found,
+                          texture_found);
 
     // Set the entity color. Use the team color by default
     std::vector<int> color;
     auto it_color = info.find("color");
-    if (it_color != info.end() and
-        str2container(it_color->second, ", ", color, 3)) {
+    if (it_color != info.end() and str2container(it_color->second, ", ", color, 3)) {
     } else {
         set(color, mp->team_info()[id_.team_id()].color);
     }
@@ -495,7 +461,8 @@ bool Entity::parse_visual(std::map<std::string, std::string> &info,
     std::string visual_model = boost::to_upper_copy(info["visual_model"]);
     if (mesh_found) {
         type_ = Contact::Type::MESH;
-        visual_->set_visual_mode(texture_found ? scrimmage_proto::ContactVisual::TEXTURE : scrimmage_proto::ContactVisual::COLOR);
+        visual_->set_visual_mode(texture_found ? scrimmage_proto::ContactVisual::TEXTURE
+                                               : scrimmage_proto::ContactVisual::COLOR);
     } else if (visual_model == std::string("QUADROTOR")) {
         type_ = Contact::Type::QUADROTOR;
         visual_->set_visual_mode(scrimmage_proto::ContactVisual::COLOR);
@@ -514,81 +481,112 @@ bool Entity::parse_visual(std::map<std::string, std::string> &info,
 }
 
 bool Entity::ready() {
-    auto all_ready = [&](auto &rng, auto &func) {
+    auto all_ready = [&](auto& rng, auto& func) {
         return std::all_of(rng.begin(), rng.end(), func);
     };
 
-    auto single_ready = [&](auto &plugin) {return plugin->ready();};
-    auto values_single_ready = [&](auto &kv) {return kv.second->ready();};
+    auto single_ready = [&](auto& plugin) { return plugin->ready(); };
+    auto values_single_ready = [&](auto& kv) { return kv.second->ready(); };
 
-    return all_ready(autonomies_, single_ready)
-        && all_ready(controllers_, single_ready)
-        && all_ready(sensors_, values_single_ready)
-        && motion_model_->ready();
+    return all_ready(autonomies_, single_ready) && all_ready(controllers_, single_ready)
+           && all_ready(sensors_, values_single_ready) && motion_model_->ready();
 }
 
-StatePtr &Entity::state() {return state_;}
-StatePtr &Entity::state_truth() {return state_truth_;}
+StatePtr& Entity::state() {
+    return state_;
+}
+StatePtr& Entity::state_truth() {
+    return state_truth_;
+}
 
-std::vector<AutonomyPtr> &Entity::autonomies() {return autonomies_;}
+std::vector<AutonomyPtr>& Entity::autonomies() {
+    return autonomies_;
+}
 
-MotionModelPtr &Entity::motion() {return motion_model_;}
+MotionModelPtr& Entity::motion() {
+    return motion_model_;
+}
 
-std::vector<ControllerPtr> &Entity::controllers() {
+std::vector<ControllerPtr>& Entity::controllers() {
     return controllers_;
 }
 
-void Entity::set_id(ID &id) { id_ = id; }
+void Entity::set_id(ID& id) {
+    id_ = id;
+}
 
-ID &Entity::id() { return id_; }
+ID& Entity::id() {
+    return id_;
+}
 
-void Entity::collision() { health_points_ -= 1e9; }
+void Entity::collision() {
+    health_points_ -= 1e9;
+}
 
-void Entity::hit() { health_points_--; }
+void Entity::hit() {
+    health_points_--;
+}
 
-void Entity::set_health_points(int health_points)
-{ health_points_ = health_points; }
+void Entity::set_health_points(int health_points) {
+    health_points_ = health_points;
+}
 
-int Entity::health_points() { return health_points_; }
+int Entity::health_points() {
+    return health_points_;
+}
 
 bool Entity::is_alive() {
     return (health_points_ > 0);
 }
 
 bool Entity::posthumous(double t) {
-    bool any_autonomies =
-        std::any_of(autonomies_.begin(), autonomies_.end(),
-                    [t](AutonomyPtr &a) {return a->posthumous(t);});
+    bool any_autonomies = std::any_of(autonomies_.begin(), autonomies_.end(),
+                                      [t](AutonomyPtr& a) { return a->posthumous(t); });
     return any_autonomies && motion_model_->posthumous(t);
 }
 
-std::shared_ptr<GeographicLib::LocalCartesian> Entity::projection()
-{ return proj_; }
+std::shared_ptr<GeographicLib::LocalCartesian> Entity::projection() {
+    return proj_;
+}
 
-MissionParsePtr Entity::mp() { return mp_; }
+MissionParsePtr Entity::mp() {
+    return mp_;
+}
 
-void Entity::set_mp(MissionParsePtr mp) { mp_ = mp; }
-void Entity::set_random(RandomPtr random) { random_ = random; }
+void Entity::set_mp(MissionParsePtr mp) {
+    mp_ = mp;
+}
+void Entity::set_random(RandomPtr random) {
+    random_ = random;
+}
 
-RandomPtr Entity::random() { return random_; }
+RandomPtr Entity::random() {
+    return random_;
+}
 
-Contact::Type Entity::type() { return type_; }
+Contact::Type Entity::type() {
+    return type_;
+}
 
-void Entity::set_visual_changed(bool visual_changed)
-{ visual_changed_ = visual_changed; }
+void Entity::set_visual_changed(bool visual_changed) {
+    visual_changed_ = visual_changed;
+}
 
-bool Entity::visual_changed() { return visual_changed_; }
+bool Entity::visual_changed() {
+    return visual_changed_;
+}
 
-scrimmage_proto::ContactVisualPtr &Entity::contact_visual()
-{ return visual_; }
+scrimmage_proto::ContactVisualPtr& Entity::contact_visual() {
+    return visual_;
+}
 
-std::unordered_map<std::string, SensorPtr> &Entity::sensors() {
+std::unordered_map<std::string, SensorPtr>& Entity::sensors() {
     return sensors_;
 }
 
-std::unordered_map<std::string, SensorPtr> Entity::sensors(const std::string &sensor_name) {
+std::unordered_map<std::string, SensorPtr> Entity::sensors(const std::string& sensor_name) {
     std::unordered_map<std::string, SensorPtr> out;
-    for (auto &kv : sensors_) {
+    for (auto& kv : sensors_) {
         if (kv.first.find(sensor_name) != std::string::npos) {
             out[kv.first] = kv.second;
         }
@@ -596,47 +594,52 @@ std::unordered_map<std::string, SensorPtr> Entity::sensors(const std::string &se
     return out;
 }
 
-SensorPtr Entity::sensor(const std::string &sensor_name) {
+SensorPtr Entity::sensor(const std::string& sensor_name) {
     std::unordered_map<std::string, SensorPtr> out = sensors(sensor_name);
     return out.empty() ? nullptr : out.begin()->second;
 }
 
-void Entity::set_active(bool active) { active_ = active; }
+void Entity::set_active(bool active) {
+    active_ = active;
+}
 
-bool Entity::active() { return active_; }
+bool Entity::active() {
+    return active_;
+}
 
 void Entity::setup_desired_state() {
     if (controllers_.empty()) return;
 
     auto it = std::find_if(autonomies_.rbegin(), autonomies_.rend(),
-        [&](auto autonomy) {return autonomy->get_is_controlling();});
+                           [&](auto autonomy) { return autonomy->get_is_controlling(); });
 
     if (it != autonomies_.rend()) {
         controllers_.front()->set_desired_state((*it)->desired_state());
     }
 }
 
-std::unordered_map<std::string, Service> &Entity::services() {return services_;}
-std::unordered_map<std::string, Service> &Entity::global_services() {
+std::unordered_map<std::string, Service>& Entity::services() {
+    return services_;
+}
+std::unordered_map<std::string, Service>& Entity::global_services() {
     return global_services_->services();
 }
 
-void Entity::set_global_services(const GlobalServicePtr &global_services) {
+void Entity::set_global_services(const GlobalServicePtr& global_services) {
     global_services_ = global_services;
 }
 
-bool Entity::call_service(scrimmage::MessageBasePtr req,
-        scrimmage::MessageBasePtr &res, const std::string &service_name) {
+bool Entity::call_service(scrimmage::MessageBasePtr req, scrimmage::MessageBasePtr& res,
+                          const std::string& service_name) {
     auto it = services_.find(service_name);
     if (it == services_.end()) {
         // First check for a global service of this name
         bool found = global_services_->call_service(req, res, service_name);
         if (!found) {
-            std::cout << "request for service ("
-                << service_name
-                << ") that does not exist" << std::endl;
+            std::cout << "request for service (" << service_name << ") that does not exist"
+                      << std::endl;
             std::cout << "services are: ";
-            for (auto &kv : services_) {
+            for (auto& kv : services_) {
                 std::cout << kv.first << ", ";
             }
             std::cout << std::endl;
@@ -646,7 +649,7 @@ bool Entity::call_service(scrimmage::MessageBasePtr req,
         }
     }
 
-    Service &service = it->second;
+    Service& service = it->second;
     bool success = service(req, res);
 
     if (!success) {
@@ -657,7 +660,7 @@ bool Entity::call_service(scrimmage::MessageBasePtr req,
     }
 }
 
-void Entity::print(const std::string &msg) {
+void Entity::print(const std::string& msg) {
     std::cout << msg << std::endl;
 }
 
@@ -666,7 +669,7 @@ void Entity::close(double t) {
         autonomy->close_plugin(t);
     }
 
-    for (auto &kv : sensors_) {
+    for (auto& kv : sensors_) {
         kv.second->close_plugin(t);
     }
 
@@ -699,22 +702,25 @@ void Entity::close(double t) {
     time_ = nullptr;
 }
 
-std::unordered_map<std::string, MessageBasePtr> &Entity::properties() {
+std::unordered_map<std::string, MessageBasePtr>& Entity::properties() {
     return properties_;
 }
 
-void Entity::set_time_ptr(TimePtr t) {time_ = t;}
-void Entity::set_printer(PrintPtr printer) { printer_ = printer; }
-
+void Entity::set_time_ptr(TimePtr t) {
+    time_ = t;
+}
+void Entity::set_printer(PrintPtr printer) {
+    printer_ = printer;
+}
 
 // cppcheck-suppress passedByValue
-void Entity::set_projection(const std::shared_ptr<GeographicLib::LocalCartesian> &proj) {
+void Entity::set_projection(const std::shared_ptr<GeographicLib::LocalCartesian>& proj) {
     proj_ = proj;
 }
 
-void Entity::print_plugins(std::ostream &out) const {
+void Entity::print_plugins(std::ostream& out) const {
     out << "----------- Sensor -------------" << endl;
-    for (auto &kv : sensors_) {
+    for (auto& kv : sensors_) {
         out << kv.second->name() << endl;
     }
     out << "---------- Autonomy ------------" << endl;
diff --git a/src/entity/EntityPlugin.cpp b/src/entity/EntityPlugin.cpp
index a5977186a2..3f21eef166 100644
--- a/src/entity/EntityPlugin.cpp
+++ b/src/entity/EntityPlugin.cpp
@@ -30,47 +30,55 @@
  *
  */
 
-#include <scrimmage/entity/EntityPlugin.h>
+#include <scrimmage/common/ParameterServer.h>
+#include <scrimmage/common/Random.h>
+#include <scrimmage/common/Time.h>
 #include <scrimmage/entity/Entity.h>
+#include <scrimmage/entity/EntityPlugin.h>
 #include <scrimmage/math/State.h>
-#include <scrimmage/pubsub/Publisher.h>
-#include <scrimmage/pubsub/PubSub.h>
-#include <scrimmage/common/Time.h>
-#include <scrimmage/common/Random.h>
-#include <scrimmage/common/ParameterServer.h>
-#include <scrimmage/proto/Shape.pb.h>
 #include <scrimmage/proto/ProtoConversions.h>
+#include <scrimmage/proto/Shape.pb.h>
+#include <scrimmage/pubsub/PubSub.h>
+#include <scrimmage/pubsub/Publisher.h>
 
-#include <string>
-#include <memory>
 #include <functional>
+#include <memory>
+#include <string>
 
 namespace scrimmage {
-EntityPlugin::EntityPlugin() : parent_(std::make_shared<Entity>()),
-                               transform_(std::make_shared<State>()),
-                               id_to_team_map_(std::make_shared<std::unordered_map<int, int>>()),
-                               id_to_ent_map_(std::make_shared<std::unordered_map<int, EntityPtr>>()),
-                               time_(std::make_shared<const Time>()),
-                               param_server_(std::make_shared<ParameterServer>()),
-                               loop_rate_(0.0),
-                               loop_timer_(0.0) {}
+EntityPlugin::EntityPlugin()
+    : parent_(std::make_shared<Entity>()),
+      transform_(std::make_shared<State>()),
+      id_to_team_map_(std::make_shared<std::unordered_map<int, int>>()),
+      id_to_ent_map_(std::make_shared<std::unordered_map<int, EntityPtr>>()),
+      time_(std::make_shared<const Time>()),
+      param_server_(std::make_shared<ParameterServer>()),
+      loop_rate_(0.0),
+      loop_timer_(0.0) {
+}
 
-EntityPlugin::~EntityPlugin() {}
+EntityPlugin::~EntityPlugin() {
+}
 
-void EntityPlugin::set_parent(EntityPtr parent) {parent_ = parent;}
+void EntityPlugin::set_parent(EntityPtr parent) {
+    parent_ = parent;
+}
 
-EntityPtr EntityPlugin::parent() { return parent_; }
+EntityPtr EntityPlugin::parent() {
+    return parent_;
+}
 
-void EntityPlugin::set_scoped_property(const std::string &property_name, const MessageBasePtr &property) {
+void EntityPlugin::set_scoped_property(const std::string& property_name,
+                                       const MessageBasePtr& property) {
     parent_->properties()[name() + "/" + property_name] = property;
 }
 
-MessageBasePtr EntityPlugin::get_scoped_property_helper(const std::string &property_name) {
+MessageBasePtr EntityPlugin::get_scoped_property_helper(const std::string& property_name) {
     auto it = parent_->properties().find(name() + "/" + property_name);
     return it == parent_->properties().end() ? nullptr : it->second;
 }
 
-std::list<scrimmage_proto::ShapePtr> &EntityPlugin::shapes() {
+std::list<scrimmage_proto::ShapePtr>& EntityPlugin::shapes() {
     return shapes_;
 }
 
@@ -89,10 +97,9 @@ void EntityPlugin::draw_shape(scrimmage_proto::ShapePtr s) {
     if (!s->hash_set()) {
         // Hash function uses entity ID, current simulation time, plugin name,
         // and a random number.
-        std::string str = name() + std::to_string(parent_->id().id())
-                + std::to_string(time_->t())
-                + std::to_string(parent_->random()->rng_uniform())
-                + std::to_string(parent_->random()->rng_uniform());
+        std::string str = name() + std::to_string(parent_->id().id()) + std::to_string(time_->t())
+                          + std::to_string(parent_->random()->rng_uniform())
+                          + std::to_string(parent_->random()->rng_uniform());
 
         std::size_t hash_id = std::hash<std::string>{}(str);
         s->set_hash(hash_id);
@@ -101,23 +108,22 @@ void EntityPlugin::draw_shape(scrimmage_proto::ShapePtr s) {
     shapes_.push_back(s);
 }
 
-void EntityPlugin::set_param_server(const ParameterServerPtr &param_server) {
+void EntityPlugin::set_param_server(const ParameterServerPtr& param_server) {
     param_server_ = param_server;
 }
 
 bool EntityPlugin::step_loop_timer(double dt) {
     loop_timer_ -= dt;
     if (loop_timer_ <= 0.0) {
-        loop_timer_ = loop_rate_ == 0 ?
-                -1.0 : loop_timer_ + (1.0 / loop_rate_);
+        loop_timer_ = loop_rate_ == 0 ? -1.0 : loop_timer_ + (1.0 / loop_rate_);
         return true;
     } else {
         return false;
     }
 }
 
-void EntityPlugin::close_plugin(const double &t) {
-    close(t); // allow subclass to close()
+void EntityPlugin::close_plugin(const double& t) {
+    close(t);  // allow subclass to close()
 
     param_server_->unregister_params(std::static_pointer_cast<EntityPlugin>(shared_from_this()));
     parent_ = nullptr;
@@ -128,4 +134,4 @@ void EntityPlugin::close_plugin(const double &t) {
     time_ = nullptr;
     shapes_.clear();
 }
-} // namespace scrimmage
+}  // namespace scrimmage
diff --git a/src/entity/External.cpp b/src/entity/External.cpp
index ed5a264851..8211587a3f 100644
--- a/src/entity/External.cpp
+++ b/src/entity/External.cpp
@@ -31,70 +31,69 @@
  */
 
 #include <scrimmage/autonomy/Autonomy.h>
-#include <scrimmage/motion/MotionModel.h>
 #include <scrimmage/common/FileSearch.h>
-#include <scrimmage/common/Random.h>
+#include <scrimmage/common/GlobalService.h>
 #include <scrimmage/common/RTree.h>
+#include <scrimmage/common/Random.h>
 #include <scrimmage/common/Time.h>
-#include <scrimmage/common/GlobalService.h>
 #include <scrimmage/entity/External.h>
 #include <scrimmage/log/Log.h>
 #include <scrimmage/metrics/Metrics.h>
 #include <scrimmage/motion/Controller.h>
+#include <scrimmage/motion/MotionModel.h>
+#include <scrimmage/network/Interface.h>
 #include <scrimmage/parse/MissionParse.h>
 #include <scrimmage/parse/ParseUtils.h>
 #include <scrimmage/plugin_manager/PluginManager.h>
 #include <scrimmage/pubsub/Network.h>
 #include <scrimmage/sensor/Sensor.h>
-#include <scrimmage/simcontrol/SimUtils.h>
-#include <scrimmage/network/Interface.h>
 #include <scrimmage/simcontrol/EntityInteraction.h>
-
-#include <iostream>
-#include <iomanip>
-#include <set>
+#include <scrimmage/simcontrol/SimUtils.h>
 
 #include <GeographicLib/Geocentric.hpp>
 #include <GeographicLib/LocalCartesian.hpp>
-
-#include <boost/range/adaptor/map.hpp>
 #include <boost/range/adaptor/filtered.hpp>
+#include <boost/range/adaptor/map.hpp>
 #include <boost/range/adaptor/transformed.hpp>
 #include <boost/range/algorithm/copy.hpp>
 #include <boost/range/algorithm/find_if.hpp>
 #include <boost/range/algorithm/for_each.hpp>
+#include <iomanip>
+#include <iostream>
+#include <set>
 
-using std::endl;
 using std::cout;
+using std::endl;
 
 namespace ba = boost::adaptors;
 namespace br = boost::range;
 
 namespace scrimmage {
 
-External::External() :
-    entity_(std::make_shared<Entity>()),
-    plugin_manager_(std::make_shared<PluginManager>()),
-    log_(std::make_shared<Log>()),
-    last_t_(NAN),
-    pubsub_(std::make_shared<PubSub>()),
-    printer_(std::make_shared<Print>()),
-    time_(std::make_shared<Time>()),
-    param_server_(std::make_shared<ParameterServer>()),
-    global_services_(std::make_shared<GlobalService>()),
-    mp_(std::make_shared<MissionParse>()),
-    id_to_team_map_(std::make_shared<std::unordered_map<int, int>>()),
-    id_to_ent_map_(std::make_shared<std::unordered_map<int, EntityPtr>>()),
-    shape_queue_max_size_(0) {}
-
-void External::print_plugins(std::ostream &out) const {
+External::External()
+    : entity_(std::make_shared<Entity>()),
+      plugin_manager_(std::make_shared<PluginManager>()),
+      log_(std::make_shared<Log>()),
+      last_t_(NAN),
+      pubsub_(std::make_shared<PubSub>()),
+      printer_(std::make_shared<Print>()),
+      time_(std::make_shared<Time>()),
+      param_server_(std::make_shared<ParameterServer>()),
+      global_services_(std::make_shared<GlobalService>()),
+      mp_(std::make_shared<MissionParse>()),
+      id_to_team_map_(std::make_shared<std::unordered_map<int, int>>()),
+      id_to_ent_map_(std::make_shared<std::unordered_map<int, EntityPtr>>()),
+      shape_queue_max_size_(0) {
+}
+
+void External::print_plugins(std::ostream& out) const {
     out << "====== SCRIMMAGE Plugins Loaded =======" << endl;
     out << "----------- Network ------------" << endl;
-    for (auto &kv : *networks_) {
+    for (auto& kv : *networks_) {
         out << kv.first << endl;
     }
     out << "------ Entity Interaction ------" << endl;
-    for (EntityInteractionPtr ei:  ent_inters_) {
+    for (EntityInteractionPtr ei : ent_inters_) {
         out << ei->name() << endl;
     }
     out << "----------- Metrics ------------" << endl;
@@ -104,17 +103,12 @@ void External::print_plugins(std::ostream &out) const {
     entity_->print_plugins(out);
 }
 
-bool External::create_entity(const std::string &mission_file,
-                             const std::string &entity_tag,
-                             const std::string &plugin_tags_str,
-                             int entity_id,
-                             int max_entities,
-                             double init_time,
-                             double init_dt,
-                             const std::string &log_dir,
-                             std::function<void(std::map<std::string, std::string>&)> param_override_func,
-                             const std::string& mission_file_overrides,
-                             const int& debug_level) {
+bool External::create_entity(
+    const std::string& mission_file, const std::string& entity_tag,
+    const std::string& plugin_tags_str, int entity_id, int max_entities, double init_time,
+    double init_dt, const std::string& log_dir,
+    std::function<void(std::map<std::string, std::string>&)> param_override_func,
+    const std::string& mission_file_overrides, const int& debug_level) {
     // Find the mission file
     auto found_mission_file = FileSearch().find_mission(mission_file);
     if (not found_mission_file) {
@@ -137,8 +131,7 @@ bool External::create_entity(const std::string &mission_file,
     // entity name
     auto it_name_id = mp_->entity_tag_to_id().find(entity_tag);
     if (it_name_id == mp_->entity_tag_to_id().end()) {
-        cout << "Entity name (" << entity_tag << ") not found in mission file"
-             << endl;
+        cout << "Entity name (" << entity_tag << ") not found in mission file" << endl;
         return false;
     }
 
@@ -187,7 +180,8 @@ bool External::create_entity(const std::string &mission_file,
     }
 
     ent_inters_.clear();
-    if (!create_ent_inters(sim_info, contacts, shapes, ent_inters_, global_services_, plugin_tags, param_override_func)) {
+    if (!create_ent_inters(sim_info, contacts, shapes, ent_inters_, global_services_, plugin_tags,
+                           param_override_func)) {
         std::cout << "External::create_entity() failed on create_ent_inters()" << std::endl;
         return false;
     }
@@ -201,20 +195,17 @@ bool External::create_entity(const std::string &mission_file,
     call_update_contacts(time_->t());
     auto it = entity_->contacts()->find(entity_id);
     if (it != entity_->contacts()->end()) {
-        mp_->entity_descriptions()[it_name_id->second]["team_id"] = std::to_string(it->second.id().team_id());
+        mp_->entity_descriptions()[it_name_id->second]["team_id"] =
+            std::to_string(it->second.id().team_id());
     }
 
-    std::map<std::string, std::string> info =
-        mp_->entity_descriptions()[it_name_id->second];
-
-    AttributeMap &attr_map = mp_->entity_attributes()[it_name_id->second];
-    bool ent_success =
-        entity_->init(
-            attr_map, info, id_to_team_map_, id_to_ent_map_, contacts, mp_,
-            mp_->projection(), entity_id,
-            it_name_id->second, plugin_manager_, file_search, rtree, pubsub_,
-            printer_, time_, param_server_, global_services_, plugin_tags,
-            param_override_func, debug_level);
+    std::map<std::string, std::string> info = mp_->entity_descriptions()[it_name_id->second];
+
+    AttributeMap& attr_map = mp_->entity_attributes()[it_name_id->second];
+    bool ent_success = entity_->init(
+        attr_map, info, id_to_team_map_, id_to_ent_map_, contacts, mp_, mp_->projection(),
+        entity_id, it_name_id->second, plugin_manager_, file_search, rtree, pubsub_, printer_,
+        time_, param_server_, global_services_, plugin_tags, param_override_func, debug_level);
     if (!ent_success) {
         std::cout << "External::create_entity() failed on entity_->init()" << std::endl;
         return false;
@@ -226,11 +217,11 @@ bool External::create_entity(const std::string &mission_file,
         // if vars is not set then we assume it should match the last controller.
         // we need to make sure we do not mess up the order of the indexes
         // because the controller outputs have already been declared
-        auto &ctrl = entity_->controllers().back();
-        auto &var_idx = ctrl->vars().output_variable_index();
+        auto& ctrl = entity_->controllers().back();
+        auto& var_idx = ctrl->vars().output_variable_index();
         if (vars.input_variable_index().empty()) {
             int idx = 0;
-            auto matches_idx = [&](auto &kv) {return kv.second == idx;};
+            auto matches_idx = [&](auto& kv) { return kv.second == idx; };
             auto it = br::find_if(var_idx, matches_idx);
             while (it != var_idx.end()) {
                 vars.add_input_variable(it->first);
@@ -241,14 +232,11 @@ bool External::create_entity(const std::string &mission_file,
         }
         connect(ctrl->vars(), vars);
         if (!verify_io_connection(entity_->controllers().back()->vars(), vars)) {
-            std::cout << "VariableIO Error: "
-                      << ctrl->name()
-                      << " does not provide inputs required by the External class."
-                      << std::endl;
+            std::cout << "VariableIO Error: " << ctrl->name()
+                      << " does not provide inputs required by the External class." << std::endl;
             print_io_error("External", vars);
             std::cout << ctrl->name() << " currently provides the following: ";
-            br::copy(var_idx | ba::map_keys,
-                     std::ostream_iterator<std::string>(std::cout, ", "));
+            br::copy(var_idx | ba::map_keys, std::ostream_iterator<std::string>(std::cout, ", "));
             return false;
         }
     }
@@ -256,15 +244,14 @@ bool External::create_entity(const std::string &mission_file,
     return true;
 }
 
-void External::setup_logging(const std::string &log_dir) {
+void External::setup_logging(const std::string& log_dir) {
     mp_->set_log_dir(log_dir);
 
     std::string output_type = get("output_type", mp_->params(), std::string("all"));
 
     mp_->create_log_dir();
-    bool enable_log =
-        output_type.find("all") != std::string::npos ||
-        output_type.find("frames") != std::string::npos;
+    bool enable_log = output_type.find("all") != std::string::npos
+                      || output_type.find("frames") != std::string::npos;
 
     log_->set_enable_log(enable_log);
     log_->set_drop_bin_logging(mp_->get_no_bin_logging());
@@ -290,7 +277,7 @@ bool External::step(double t) {
         autonomy->step_autonomy(t, dt);
     }
 
-    for (const auto &kv : entity_->sensors()) {
+    for (const auto& kv : entity_->sensors()) {
         SensorPtr sensor = kv.second;
         sensor->step();
     }
@@ -320,13 +307,16 @@ bool External::step(double t) {
         metrics->step_metrics(t, dt);
     }
 
-    if (!send_messages()) {mutex.unlock(); return false;}
+    if (!send_messages()) {
+        mutex.unlock();
+        return false;
+    }
 
     // shapes
     scrimmage_proto::Shapes shapes;
     shapes.set_time(t);
     auto add_clear_shapes = [&](auto p) {
-        auto add_shape = [&](auto &shape) {
+        auto add_shape = [&](auto& shape) {
             *shapes.add_shape() = *shape;
 
             shape_queue_.push(shape);
@@ -337,7 +327,7 @@ bool External::step(double t) {
             }
         };
         br::for_each(p->shapes(), add_shape);
-        p->shapes().clear(); // don't let shapes build up
+        p->shapes().clear();  // don't let shapes build up
     };
 
     br::for_each(entity_->autonomies(), add_clear_shapes);
@@ -359,21 +349,21 @@ bool External::step(double t) {
     return true;
 }
 
-EntityPtr &External::entity() {
+EntityPtr& External::entity() {
     return entity_;
 }
 
 bool External::send_messages() {
     // Send messages that are published by a SCRIMMAGE plugin to the external
     // network (e.g., ROS, MOOS)
-    auto to_publisher = [&](auto &network_device) {return std::dynamic_pointer_cast<Publisher>(network_device);};
-    auto has_callback = [&](auto &pub) {return pub && pub->callback;};
-
-    for (auto &topic_device_kv : pubsub_->pubs() | ba::map_values) {
-        for (auto &dev_list : topic_device_kv | ba::map_values) {
-            for (auto pub : dev_list |
-                            ba::transformed(to_publisher) |
-                            ba::filtered(has_callback)) {
+    auto to_publisher = [&](auto& network_device) {
+        return std::dynamic_pointer_cast<Publisher>(network_device);
+    };
+    auto has_callback = [&](auto& pub) { return pub && pub->callback; };
+
+    for (auto& topic_device_kv : pubsub_->pubs() | ba::map_values) {
+        for (auto& dev_list : topic_device_kv | ba::map_values) {
+            for (auto pub : dev_list | ba::transformed(to_publisher) | ba::filtered(has_callback)) {
                 for (auto msg : pub->pop_msgs()) {
                     pub->callback(msg);
                 }
@@ -383,9 +373,9 @@ bool External::send_messages() {
 
     // any messages that are not linked to the external network can now
     // be sent via the internal scrimmage messaging system
-    auto &pubs = pubsub_->pubs();
-    auto &subs = pubsub_->subs();
-    for (auto &network : *networks_ | ba::map_values) {
+    auto& pubs = pubsub_->pubs();
+    auto& subs = pubsub_->subs();
+    for (auto& network : *networks_ | ba::map_values) {
         auto name = network->name();
         if (!network->step(pubs[name], subs[name])) {
             std::cout << "Network failed: " << name << std::endl;
@@ -404,10 +394,13 @@ bool External::send_messages() {
 bool External::call_update_contacts(double t) {
     mutex.lock();
     if (update_contacts_task.update(t).first) {
-        auto rtree = entity_->rtree(); // rtree is a shared_ptr
-        if (!rtree) {mutex.unlock(); return false;}
+        auto rtree = entity_->rtree();  // rtree is a shared_ptr
+        if (!rtree) {
+            mutex.unlock();
+            return false;
+        }
         rtree->init(entity_->contacts()->size());
-        for (auto &kv : *entity_->contacts()) {
+        for (auto& kv : *entity_->contacts()) {
             rtree->add(kv.second.state()->pos(), kv.second.id());
         }
         update_ents();
@@ -423,13 +416,13 @@ MissionParsePtr External::mp() {
 void External::update_ents() {
     if (ent_inters_.empty() && !metrics_.empty()) return;
 
-    auto &contacts = *entity_->contacts();
+    auto& contacts = *entity_->contacts();
 
     id_to_team_map_->clear();
     id_to_ent_map_->clear();
     ents_.clear();
-    for (auto &kv : contacts) {
-        ID &id = kv.second.id();
+    for (auto& kv : contacts) {
+        ID& id = kv.second.id();
         (*id_to_team_map_)[id.id()] = id.team_id();
 
         auto ent = id.id() == entity_->id().id() ? entity_ : std::make_shared<Entity>();
@@ -450,13 +443,13 @@ void External::close() {
     entity_->close(time_->t());
     entity_ = std::make_shared<Entity>();
 
-    auto close = [&](auto p) {p->close(time_->t());};
+    auto close = [&](auto p) { p->close(time_->t()); };
     br::for_each(ent_inters_, close);
     br::for_each(metrics_, close);
     mutex.unlock();
 }
 
-bool External::enable_outgoing_interface(const std::string &ip, const int &port) {
+bool External::enable_outgoing_interface(const std::string& ip, const int& port) {
     InterfacePtr interface = std::make_shared<Interface>();
     interface->init_network(Interface::client, ip, port);
 
@@ -470,9 +463,9 @@ bool External::disable_outgoing_interfaces() {
     return true;
 }
 
-void External::set_time(const double &t, const double &dt) {
+void External::set_time(const double& t, const double& dt) {
     time_->set_t(t);
     time_->set_dt(dt);
 }
 
-} // namespace scrimmage
+}  // namespace scrimmage
diff --git a/src/log/FrameUpdateClient.cpp b/src/log/FrameUpdateClient.cpp
index 7b1a5f84aa..129c5aa41d 100644
--- a/src/log/FrameUpdateClient.cpp
+++ b/src/log/FrameUpdateClient.cpp
@@ -35,17 +35,16 @@
 #include <scrimmage/proto/Scrimmage.grpc.pb.h>
 #endif
 
-#include <scrimmage/log/FrameUpdateClient.h>
 #include <scrimmage/common/Utilities.h>
+#include <scrimmage/log/FrameUpdateClient.h>
 #include <scrimmage/math/Angles.h>
 #include <scrimmage/math/Quaternion.h>
 
+#include <GeographicLib/LocalCartesian.hpp>
 #include <iostream>
 #include <memory>
 #include <string>
 
-#include <GeographicLib/LocalCartesian.hpp>
-
 #if ENABLE_GRPC == 1
 using grpc::Channel;
 using grpc::ClientContext;
@@ -57,11 +56,11 @@ using std::endl;
 
 namespace scrimmage {
 
-FrameUpdateClient::FrameUpdateClient(const std::string &ip, int port) :
-    angles_to_gps_(0, Angles::Type::EUCLIDEAN, Angles::Type::GPS) {}
-
-bool FrameUpdateClient::send_frame(scrimmage_proto::Frame &frame) {
+FrameUpdateClient::FrameUpdateClient(const std::string& ip, int port)
+    : angles_to_gps_(0, Angles::Type::EUCLIDEAN, Angles::Type::GPS) {
+}
 
+bool FrameUpdateClient::send_frame(scrimmage_proto::Frame& frame) {
     // Change from local x, y, z to GPS coordinates:
     for (int i = 0; i < frame.contact_size(); i++) {
         // frame.contacts_[frame.contact(i).id().id()] = proto_2_contact(proto_frame.contact(i));
@@ -119,7 +118,7 @@ bool FrameUpdateClient::send_frame(scrimmage_proto::Frame &frame) {
     return true;
 }
 
-void FrameUpdateClient::set_projection(const std::shared_ptr<GeographicLib::LocalCartesian> &proj) {
+void FrameUpdateClient::set_projection(const std::shared_ptr<GeographicLib::LocalCartesian>& proj) {
     proj_ = proj;
 }
-} // namespace scrimmage
+}  // namespace scrimmage
diff --git a/src/log/Log.cpp b/src/log/Log.cpp
index 4217ca171a..432a22d22f 100644
--- a/src/log/Log.cpp
+++ b/src/log/Log.cpp
@@ -31,27 +31,23 @@
  */
 
 #include <fcntl.h>
-#include <unistd.h>
-
-#include <google/protobuf/message_lite.h>
-#include <google/protobuf/io/zero_copy_stream_impl_lite.h>
-#include <google/protobuf/io/zero_copy_stream_impl.h>
 #include <google/protobuf/io/coded_stream.h>
-
+#include <google/protobuf/io/zero_copy_stream_impl.h>
+#include <google/protobuf/io/zero_copy_stream_impl_lite.h>
+#include <google/protobuf/message_lite.h>
+#include <scrimmage/entity/EntityPlugin.h>
+#include <scrimmage/log/Log.h>
+#include <scrimmage/math/State.h>
+#include <scrimmage/msgs/Collision.pb.h>
 #include <scrimmage/proto/Frame.pb.h>
+#include <scrimmage/proto/ProtoConversions.h>
 #include <scrimmage/proto/Shape.pb.h>
 #include <scrimmage/proto/Visual.pb.h>
-#include <scrimmage/math/State.h>
-#include <scrimmage/log/Log.h>
-#include <scrimmage/proto/ProtoConversions.h>
-#include <scrimmage/entity/EntityPlugin.h>
-
 #include <scrimmage/pubsub/Message.h>
-#include <scrimmage/msgs/Collision.pb.h>
-
-#include <iostream>
+#include <unistd.h>
 
 #include <boost/filesystem.hpp>
+#include <iostream>
 namespace fs = boost::filesystem;
 
 using std::cout;
@@ -61,8 +57,7 @@ namespace sp = scrimmage_proto;
 
 namespace scrimmage {
 
-Log::Log() : frames_output_(), shapes_output_(), utm_terrain_output_(),
-             contact_visual_output_() {
+Log::Log() : frames_output_(), shapes_output_(), utm_terrain_output_(), contact_visual_output_() {
     // Verify that the version of the library that we linked against is
     // compatible with the version of the headers we compiled against.
     GOOGLE_PROTOBUF_VERIFY_VERSION;
@@ -74,17 +69,16 @@ Log::Log() : frames_output_(), shapes_output_(), utm_terrain_output_(),
     msgs_fd_ = -1;
 }
 
-bool Log::open_file(std::string filename, int &fd) {
+bool Log::open_file(std::string filename, int& fd) {
     fd = open(filename.c_str(), O_CREAT | O_RDWR | O_TRUNC, 0644);
     if (fd == -1) {
-        cout << "Failed to open file for writing: "
-             << filename << endl;
+        cout << "Failed to open file for writing: " << filename << endl;
         return false;
     }
     return true;
 }
 
-bool Log::init(const std::string &dir, Log::Mode mode) {
+bool Log::init(const std::string& dir, Log::Mode mode) {
     mode_ = mode;
     log_dir_ = dir;
 
@@ -104,10 +98,12 @@ bool Log::init(const std::string &dir, Log::Mode mode) {
             shapes_output_ = std::make_shared<google::protobuf::io::FileOutputStream>(shapes_fd_);
         }
         if (open_file(utm_terrain_name_, utm_terrain_fd_)) {
-            utm_terrain_output_ = std::make_shared<google::protobuf::io::FileOutputStream>(utm_terrain_fd_);
+            utm_terrain_output_ =
+                std::make_shared<google::protobuf::io::FileOutputStream>(utm_terrain_fd_);
         }
         if (open_file(contact_visual_name_, contact_visual_fd_)) {
-            contact_visual_output_ = std::make_shared<google::protobuf::io::FileOutputStream>(contact_visual_fd_);
+            contact_visual_output_ =
+                std::make_shared<google::protobuf::io::FileOutputStream>(contact_visual_fd_);
         }
     } else if (mode_ == READ) {
         parse(dir);
@@ -116,12 +112,11 @@ bool Log::init(const std::string &dir, Log::Mode mode) {
     return true;
 }
 
-bool Log::write_ascii(const std::string &str) {
+bool Log::write_ascii(const std::string& str) {
     if (!enable_log_) return true;
 
     if (!ascii_output_.is_open()) {
-        ascii_output_.open(ascii_filename_.c_str(),
-                           std::ios::out | std::ios::in | std::ios::trunc);
+        ascii_output_.open(ascii_filename_.c_str(), std::ios::out | std::ios::in | std::ios::trunc);
     }
 
     if (!ascii_output_.is_open()) {
@@ -133,35 +128,50 @@ bool Log::write_ascii(const std::string &str) {
     return true;
 }
 
-bool Log::save_frame(const std::shared_ptr<scrimmage_proto::Frame> &frame) {
+bool Log::save_frame(const std::shared_ptr<scrimmage_proto::Frame>& frame) {
     return writeDelimitedTo(*frame, frames_output_);
 }
 
-bool Log::save_shapes(const scrimmage_proto::Shapes &shapes) {
+bool Log::save_shapes(const scrimmage_proto::Shapes& shapes) {
     return writeDelimitedTo(shapes, shapes_output_);
 }
 
-bool Log::save_utm_terrain(const std::shared_ptr<scrimmage_proto::UTMTerrain> &utm_terrain) {
+bool Log::save_utm_terrain(const std::shared_ptr<scrimmage_proto::UTMTerrain>& utm_terrain) {
     return writeDelimitedTo(*utm_terrain, utm_terrain_output_);
 }
 
-bool Log::save_contact_visual(const std::shared_ptr<scrimmage_proto::ContactVisual> &contact_visual) {
+bool Log::save_contact_visual(
+    const std::shared_ptr<scrimmage_proto::ContactVisual>& contact_visual) {
     return writeDelimitedTo(*contact_visual, contact_visual_output_);
 }
 
-std::string Log::frames_filename() { return frames_name_; }
+std::string Log::frames_filename() {
+    return frames_name_;
+}
 
-std::string Log::shapes_filename() { return shapes_name_; }
+std::string Log::shapes_filename() {
+    return shapes_name_;
+}
 
-std::string Log::utm_terrain_filename() { return utm_terrain_name_; }
+std::string Log::utm_terrain_filename() {
+    return utm_terrain_name_;
+}
 
-std::string Log::contact_visual_filename() { return contact_visual_name_; }
+std::string Log::contact_visual_filename() {
+    return contact_visual_name_;
+}
 
-std::string Log::msgs_filename() { return msgs_name_; }
+std::string Log::msgs_filename() {
+    return msgs_name_;
+}
 
-void Log::set_enable_log(bool enable) { enable_log_ = enable; }
+void Log::set_enable_log(bool enable) {
+    enable_log_ = enable;
+}
 
-void Log::set_drop_bin_logging(bool enable) { drop_bin_logging_ = enable; }
+void Log::set_drop_bin_logging(bool enable) {
+    drop_bin_logging_ = enable;
+}
 
 bool Log::parse(std::string dir) {
     if (!fs::is_directory(dir)) {
@@ -199,21 +209,18 @@ bool Log::parse(std::string dir) {
 }
 
 bool Log::parse(std::string filename, Log::FileType type) {
-
     if (!fs::exists(fs::path(filename))) {
         std::string path = fs::path(filename).parent_path().string();
         bool success = false;
         if (type == FRAMES) {
-            success = parse_proto(
-                [&]() {return std::make_shared<sp::Frame>();}, frames_, path, "frame"
-            );
-            for (auto &frame : frames_) {
+            success = parse_proto([&]() { return std::make_shared<sp::Frame>(); }, frames_, path,
+                                  "frame");
+            for (auto& frame : frames_) {
                 scrimmage_frames_.push_back(proto_2_frame(*frame));
             }
         } else {
-            success = parse_proto(
-                [&]() {return std::make_shared<sp::Shapes>();}, shapes_, path, "shapes"
-            );
+            success = parse_proto([&]() { return std::make_shared<sp::Shapes>(); }, shapes_, path,
+                                  "shapes");
         }
         return success;
     }
@@ -224,11 +231,11 @@ bool Log::parse(std::string filename, Log::FileType type) {
         return false;
     }
 
-    ZeroCopyInputStreamPtr input = std::make_shared<google::protobuf::io::FileInputStream>(input_fd);
+    ZeroCopyInputStreamPtr input =
+        std::make_shared<google::protobuf::io::FileInputStream>(input_fd);
 
     if (!input) {
-        cout << "Failed to open FileInputStream. Filename: "
-             << filename.c_str() << endl;
+        cout << "Failed to open FileInputStream. Filename: " << filename.c_str() << endl;
     }
 
     if (type == FRAMES) {
@@ -250,8 +257,7 @@ bool Log::parse(std::string filename, Log::FileType type) {
     return true;
 }
 
-bool Log::parse_frames(std::string filename,
-                       ZeroCopyInputStreamPtr input) {
+bool Log::parse_frames(std::string filename, ZeroCopyInputStreamPtr input) {
     frames_.clear();
     scrimmage_frames_.clear();
     bool success = false, clean_eof = false;
@@ -269,12 +275,12 @@ bool Log::parse_frames(std::string filename,
     return true;
 }
 
-bool Log::parse_shapes(std::string filename,
-                      ZeroCopyInputStreamPtr input) {
+bool Log::parse_shapes(std::string filename, ZeroCopyInputStreamPtr input) {
     shapes_.clear();
     bool success = false, clean_eof = false;
     do {
-        std::shared_ptr<scrimmage_proto::Shapes> shapes = std::make_shared<scrimmage_proto::Shapes>();
+        std::shared_ptr<scrimmage_proto::Shapes> shapes =
+            std::make_shared<scrimmage_proto::Shapes>();
         success = this->readDelimitedFrom(filename, input, shapes, clean_eof);
         if (clean_eof || !success) break;
         shapes_.push_back(shapes);
@@ -286,12 +292,12 @@ bool Log::parse_shapes(std::string filename,
     return true;
 }
 
-bool Log::parse_utm_terrain(std::string filename,
-                            ZeroCopyInputStreamPtr input) {
+bool Log::parse_utm_terrain(std::string filename, ZeroCopyInputStreamPtr input) {
     utm_terrain_.clear();
     bool success = false, clean_eof = false;
     do {
-        std::shared_ptr<scrimmage_proto::UTMTerrain> utm_terrain = std::make_shared<scrimmage_proto::UTMTerrain>();
+        std::shared_ptr<scrimmage_proto::UTMTerrain> utm_terrain =
+            std::make_shared<scrimmage_proto::UTMTerrain>();
         success = this->readDelimitedFrom(filename, input, utm_terrain, clean_eof);
         if (clean_eof || !success) break;
         utm_terrain_.push_back(utm_terrain);
@@ -303,12 +309,12 @@ bool Log::parse_utm_terrain(std::string filename,
     return true;
 }
 
-bool Log::parse_contact_visual(std::string filename,
-                               ZeroCopyInputStreamPtr input) {
+bool Log::parse_contact_visual(std::string filename, ZeroCopyInputStreamPtr input) {
     contact_visual_.clear();
     bool success = false, clean_eof = false;
     do {
-        std::shared_ptr<scrimmage_proto::ContactVisual> contact_visual = std::make_shared<scrimmage_proto::ContactVisual>();
+        std::shared_ptr<scrimmage_proto::ContactVisual> contact_visual =
+            std::make_shared<scrimmage_proto::ContactVisual>();
         success = this->readDelimitedFrom(filename, input, contact_visual, clean_eof);
         if (clean_eof || !success) break;
         contact_visual_.push_back(contact_visual);
@@ -351,8 +357,7 @@ bool Log::writeDelimitedTo(const google::protobuf::MessageLite& message,
     return true;
 }
 
-bool Log::readDelimitedFrom(const std::string &filename,
-                            ZeroCopyInputStreamPtr rawInput,
+bool Log::readDelimitedFrom(const std::string& filename, ZeroCopyInputStreamPtr rawInput,
                             MessageLitePtr message, bool& clean_eof) {
     // see here for the implementation: http://stackoverflow.com/a/22927149
 
@@ -420,16 +425,28 @@ bool Log::close_log() {
     return true;
 }
 
-std::list<Frame> &Log::scrimmage_frames() { return scrimmage_frames_; }
+std::list<Frame>& Log::scrimmage_frames() {
+    return scrimmage_frames_;
+}
 
-std::list<std::shared_ptr<scrimmage_proto::Frame> > &Log::frames() { return frames_; }
+std::list<std::shared_ptr<scrimmage_proto::Frame> >& Log::frames() {
+    return frames_;
+}
 
-std::list<std::shared_ptr<scrimmage_proto::Shapes> > &Log::shapes() { return shapes_; }
+std::list<std::shared_ptr<scrimmage_proto::Shapes> >& Log::shapes() {
+    return shapes_;
+}
 
-std::list<std::shared_ptr<scrimmage_proto::UTMTerrain> > &Log::utm_terrain() { return utm_terrain_; }
+std::list<std::shared_ptr<scrimmage_proto::UTMTerrain> >& Log::utm_terrain() {
+    return utm_terrain_;
+}
 
-std::list<std::shared_ptr<scrimmage_proto::ContactVisual> > &Log::contact_visual() { return contact_visual_; }
+std::list<std::shared_ptr<scrimmage_proto::ContactVisual> >& Log::contact_visual() {
+    return contact_visual_;
+}
 
-std::string Log::log_dir() { return log_dir_; }
+std::string Log::log_dir() {
+    return log_dir_;
+}
 
-} // namespace scrimmage
+}  // namespace scrimmage
diff --git a/src/log/Print.cpp b/src/log/Print.cpp
index 895c66b934..656aadde8f 100644
--- a/src/log/Print.cpp
+++ b/src/log/Print.cpp
@@ -63,175 +63,155 @@
  * ---------------------------------------------------------------------------
  */
 
-#include <scrimmage/log/Print.h>
-
 #include <scrimmage/common/Time.h>
-#include <scrimmage/entity/EntityPlugin.h>
 #include <scrimmage/entity/Entity.h>
+#include <scrimmage/entity/EntityPlugin.h>
+#include <scrimmage/log/Print.h>
 
-#include <iostream>
-#include <iomanip>
-#include <sstream>
 #include <fstream>
-#include <ostream>
-#include <memory>
+#include <iomanip>
+#include <iostream>
 #include <map>
-
+#include <memory>
+#include <ostream>
+#include <sstream>
 
 namespace scrimmage {
 
-  void Print::init(TimePtr &time, std::string log_dir) {
-      init(time, log_dir, PrintEnums::WRITE_TO::BOTH);
-  }
-  void Print::init(TimePtr &time, std::string log_dir, PrintEnums::WRITE_TO flag) {
-      time_ = time;
-      log_dir_ = log_dir;
-      output_flag_ = flag;
-
-      if ( write_file() ) {
-          std::ostringstream filename;
-          filename << log_dir_ << '/' << "console.log";
-           log_stream_.open(filename.str());
-      }
-  }
-  void Print::close() {
-      if ( log_stream_ ) {
-          log_stream_.close();
-      }
-      time_ = nullptr;
-  }
-  void Print::flush() {
-      if (log_stream_) {
-          log_stream_.flush();
-      }
-  }
-
-
-
-
-
-  void Print::printDev(PrintData &data, std::string outmessage) {
-      print(std::cout, PrintEnums::WARN_LEVEL::DEV, data, outmessage);
-  }
-  void Print::printInfo(PrintData &data, std::string outmessage) {
-      print(std::cout, PrintEnums::WARN_LEVEL::INFO, data, outmessage);
-  }
-  void Print::printWarning(PrintData &data, std::string outmessage) {
-      print(std::cout, PrintEnums::WARN_LEVEL::WARNING, data, outmessage);
-  }
-  void Print::printError(PrintData &data, std::string outmessage) {
-      print(std::cerr, PrintEnums::WARN_LEVEL::ERROR, data, outmessage);
-  }
-
-
-  void Print::printDev(EntityPlugin &caller, std::string outmessage) {
-      print(std::cout, PrintEnums::WARN_LEVEL::DEV, caller, outmessage);
-  }
-  void Print::printInfo(EntityPlugin &caller, std::string outmessage) {
-      print(std::cout, PrintEnums::WARN_LEVEL::INFO, caller, outmessage);
-  }
-  void Print::printWarning(EntityPlugin &caller, std::string outmessage) {
-      print(std::cerr, PrintEnums::WARN_LEVEL::WARNING, caller, outmessage);
-  }
-  void Print::printError(EntityPlugin &caller, std::string outmessage) {
-      print(std::cerr, PrintEnums::WARN_LEVEL::ERROR, caller, outmessage);
-  }
-
-
-
-
-
-  const char* Print::getWarningLevel(PrintEnums::WARN_LEVEL e)
-  {
-      const std::map<PrintEnums::WARN_LEVEL, const char*> GetAsStrings {
-          { PrintEnums::WARN_LEVEL::DEV,     "DEV" },
-          { PrintEnums::WARN_LEVEL::INFO,    "INFO" },
-          { PrintEnums::WARN_LEVEL::WARNING, "WARN" },
-          { PrintEnums::WARN_LEVEL::ERROR,   "ERROR" }
-      };
-      auto   it  = GetAsStrings.find(e);
-      return it == GetAsStrings.end() ? GetAsStrings.begin()->second : it->second;
-  }
-  bool Print::write_file() {
-    return ( (output_flag_ == PrintEnums::WRITE_TO::BOTH) || (output_flag_ == PrintEnums::WRITE_TO::FILE_ONLY) );
-  }
-  bool Print::write_console() {
-    return ( (output_flag_ == PrintEnums::WRITE_TO::BOTH) || (output_flag_ == PrintEnums::WRITE_TO::CONSOLE_ONLY) );
-  }
-
-
-
-
-
-
-
-  std::string Print::formatTime(double time) {
-      if (std::isnan( time )) {
-          return "-----.---";
-      }
-      else {
-          std::ostringstream o;
-          o << std::internal << std::fixed << std::setprecision(3)
-            << std::setw(9) << std::setfill('0') << time;
-          return o.str();
-      }
-  }
-
-
-  std::string Print::formatMsg(PrintEnums::WARN_LEVEL level, PrintData &data, std::string msg) {
-      std::ostringstream pre;
-      std::ostringstream formattedmsg;
-      std::string entitySection = "";
-
-      // add entity_id if available
-      if (data.entity_id_ > 0) {
-          entitySection = "[Entity: " + std::to_string(data.entity_id_) + "]";
-      }
-
-      // create our prefix
-      pre << formatTime(data.time_) << "s [" << data.name_ << "]" << entitySection << "[" << Print::getWarningLevel(level) << "]: ";
-
-      // remove last character if it's a newline
-      if (msg.back() == '\n') {
-          msg.pop_back(); 
-      }
-
-      // iterate and place the prefix at the beginning of each newline
-      formattedmsg << pre.str();
-      for (auto i = msg.cbegin() ; i != msg.cend() ; ++i) {
-          formattedmsg << *i;
-          // add prefix after each newline
-          if (*i == '\n') {
-              formattedmsg << pre.str();
-          }
-      }
-
-      return formattedmsg.str();
-  }
-
-
-
-
-
-
-
-  void Print::print(std::ostream& stream, PrintEnums::WARN_LEVEL level, EntityPlugin &caller, std::string msg) {
-      PrintData pd;
-      pd.time_ = caller.getTime()->t();
-      pd.name_ = caller.name();
-      pd.entity_id_ = caller.parent()->id().id();
-
-      print(stream, level, pd, msg);
-  }
-  void Print::print(std::ostream& stream, PrintEnums::WARN_LEVEL level, PrintData &data, std::string msg) {
-      std::ostringstream o;
-
-      o << formatMsg(level, data, msg) << std::endl;
-
-      if ( write_file() ) { log_stream_ << o.str(); } // write to log file
-      if ( write_console() ) { stream << o.str(); } // write to console
- }
-
-
-} // namespace scrimmage
-
+void Print::init(TimePtr& time, std::string log_dir) {
+    init(time, log_dir, PrintEnums::WRITE_TO::BOTH);
+}
+void Print::init(TimePtr& time, std::string log_dir, PrintEnums::WRITE_TO flag) {
+    time_ = time;
+    log_dir_ = log_dir;
+    output_flag_ = flag;
+
+    if (write_file()) {
+        std::ostringstream filename;
+        filename << log_dir_ << '/' << "console.log";
+        log_stream_.open(filename.str());
+    }
+}
+void Print::close() {
+    if (log_stream_) {
+        log_stream_.close();
+    }
+    time_ = nullptr;
+}
+void Print::flush() {
+    if (log_stream_) {
+        log_stream_.flush();
+    }
+}
+
+void Print::printDev(PrintData& data, std::string outmessage) {
+    print(std::cout, PrintEnums::WARN_LEVEL::DEV, data, outmessage);
+}
+void Print::printInfo(PrintData& data, std::string outmessage) {
+    print(std::cout, PrintEnums::WARN_LEVEL::INFO, data, outmessage);
+}
+void Print::printWarning(PrintData& data, std::string outmessage) {
+    print(std::cout, PrintEnums::WARN_LEVEL::WARNING, data, outmessage);
+}
+void Print::printError(PrintData& data, std::string outmessage) {
+    print(std::cerr, PrintEnums::WARN_LEVEL::ERROR, data, outmessage);
+}
+
+void Print::printDev(EntityPlugin& caller, std::string outmessage) {
+    print(std::cout, PrintEnums::WARN_LEVEL::DEV, caller, outmessage);
+}
+void Print::printInfo(EntityPlugin& caller, std::string outmessage) {
+    print(std::cout, PrintEnums::WARN_LEVEL::INFO, caller, outmessage);
+}
+void Print::printWarning(EntityPlugin& caller, std::string outmessage) {
+    print(std::cerr, PrintEnums::WARN_LEVEL::WARNING, caller, outmessage);
+}
+void Print::printError(EntityPlugin& caller, std::string outmessage) {
+    print(std::cerr, PrintEnums::WARN_LEVEL::ERROR, caller, outmessage);
+}
+
+const char* Print::getWarningLevel(PrintEnums::WARN_LEVEL e) {
+    const std::map<PrintEnums::WARN_LEVEL, const char*> GetAsStrings{
+        {PrintEnums::WARN_LEVEL::DEV, "DEV"},
+        {PrintEnums::WARN_LEVEL::INFO, "INFO"},
+        {PrintEnums::WARN_LEVEL::WARNING, "WARN"},
+        {PrintEnums::WARN_LEVEL::ERROR, "ERROR"}};
+    auto it = GetAsStrings.find(e);
+    return it == GetAsStrings.end() ? GetAsStrings.begin()->second : it->second;
+}
+bool Print::write_file() {
+    return ((output_flag_ == PrintEnums::WRITE_TO::BOTH)
+            || (output_flag_ == PrintEnums::WRITE_TO::FILE_ONLY));
+}
+bool Print::write_console() {
+    return ((output_flag_ == PrintEnums::WRITE_TO::BOTH)
+            || (output_flag_ == PrintEnums::WRITE_TO::CONSOLE_ONLY));
+}
+
+std::string Print::formatTime(double time) {
+    if (std::isnan(time)) {
+        return "-----.---";
+    } else {
+        std::ostringstream o;
+        o << std::internal << std::fixed << std::setprecision(3) << std::setw(9)
+          << std::setfill('0') << time;
+        return o.str();
+    }
+}
+
+std::string Print::formatMsg(PrintEnums::WARN_LEVEL level, PrintData& data, std::string msg) {
+    std::ostringstream pre;
+    std::ostringstream formattedmsg;
+    std::string entitySection = "";
+
+    // add entity_id if available
+    if (data.entity_id_ > 0) {
+        entitySection = "[Entity: " + std::to_string(data.entity_id_) + "]";
+    }
+
+    // create our prefix
+    pre << formatTime(data.time_) << "s [" << data.name_ << "]" << entitySection << "["
+        << Print::getWarningLevel(level) << "]: ";
+
+    // remove last character if it's a newline
+    if (msg.back() == '\n') {
+        msg.pop_back();
+    }
+
+    // iterate and place the prefix at the beginning of each newline
+    formattedmsg << pre.str();
+    for (auto i = msg.cbegin(); i != msg.cend(); ++i) {
+        formattedmsg << *i;
+        // add prefix after each newline
+        if (*i == '\n') {
+            formattedmsg << pre.str();
+        }
+    }
+
+    return formattedmsg.str();
+}
+
+void Print::print(std::ostream& stream, PrintEnums::WARN_LEVEL level, EntityPlugin& caller,
+                  std::string msg) {
+    PrintData pd;
+    pd.time_ = caller.getTime()->t();
+    pd.name_ = caller.name();
+    pd.entity_id_ = caller.parent()->id().id();
+
+    print(stream, level, pd, msg);
+}
+void Print::print(std::ostream& stream, PrintEnums::WARN_LEVEL level, PrintData& data,
+                  std::string msg) {
+    std::ostringstream o;
+
+    o << formatMsg(level, data, msg) << std::endl;
+
+    if (write_file()) {
+        log_stream_ << o.str();
+    }  // write to log file
+    if (write_console()) {
+        stream << o.str();
+    }  // write to console
+}
+
+}  // namespace scrimmage
diff --git a/src/math/Angles.cpp b/src/math/Angles.cpp
index df3a1b28f7..5b26109126 100644
--- a/src/math/Angles.cpp
+++ b/src/math/Angles.cpp
@@ -32,16 +32,15 @@
 
 #include <scrimmage/math/Angles.h>
 
-#include <cmath>
-
 #include <boost/math/special_functions/sign.hpp>
+#include <cmath>
 
 namespace scrimmage {
 
-Angles::Angles() {}
+Angles::Angles() {
+}
 
 Angles::Angles(double angle, Type input_type, Type output_type) {
-
     if (input_type == Type::GPS) {
         in_zero_ = HeadingZero::Pos_Y;
         in_direction_ = Rotate::CW;
@@ -61,9 +60,13 @@ Angles::Angles(double angle, Type input_type, Type output_type) {
     set_angle(angle);
 }
 
-double Angles::deg2rad(double deg) {return deg * M_PI / 180.0;}
+double Angles::deg2rad(double deg) {
+    return deg * M_PI / 180.0;
+}
 
-double Angles::rad2deg(double rad) {return rad * 180.0 / M_PI;}
+double Angles::rad2deg(double rad) {
+    return rad * 180.0 / M_PI;
+}
 
 void Angles::set_input_clock_direction(Angles::Rotate direction) {
     in_direction_ = direction;
@@ -91,11 +94,11 @@ bool Angles::is_angle_360(double angle) {
 
 double Angles::angle_360(double angle) {
     if (angle >= 360) {
-        angle -= 360*floor(angle/360.0);
+        angle -= 360 * floor(angle / 360.0);
     }
 
     if (angle < 0) {
-        angle += 360*ceil(std::abs(angle)/360.0);
+        angle += 360 * ceil(std::abs(angle) / 360.0);
     }
     return angle;
 }
@@ -107,7 +110,7 @@ double Angles::angle_2pi(double angle) {
     }
 
     if (angle < 0) {
-        angle += pi2 * ceil(std::abs(angle)/ pi2);
+        angle += pi2 * ceil(std::abs(angle) / pi2);
     }
 
     return angle;
@@ -157,7 +160,7 @@ void Angles::set_angle(double angle) {
         // If the angle increase is in the same direction
         // Just add or subtract a specific offset
         int rots = static_cast<int>(in_zero_) - static_cast<int>(out_zero_);
-        angle_ = angle_360(angle + rots*90);
+        angle_ = angle_360(angle + rots * 90);
     } else {
         // If the coordinates are in different CW / CCW frames
 
@@ -176,7 +179,7 @@ void Angles::set_angle(double angle) {
         if (rots == 0) {
             rots = 4;
         }
-        angle_ = angle_360(rots*90 - angle);
+        angle_ = angle_360(rots * 90 - angle);
     }
 }
 
@@ -187,14 +190,12 @@ double Angles::angle() {
 bool Angles::angle_within(double ang1, double ang2, double ang) {
     double diff = angle_diff(ang1, ang2);
     double diff1 = angle_diff(ang, ang2);
-    return boost::math::sign(diff) == boost::math::sign(diff1) &&
-        std::abs(diff) > std::abs(diff1);
+    return boost::math::sign(diff) == boost::math::sign(diff1) && std::abs(diff) > std::abs(diff1);
 }
 
 bool Angles::angle_within_rad(double ang1, double ang2, double ang) {
     double diff = angle_diff_rad(ang1, ang2);
     double diff1 = angle_diff_rad(ang, ang2);
-    return boost::math::sign(diff) == boost::math::sign(diff1) &&
-        std::abs(diff) > std::abs(diff1);
+    return boost::math::sign(diff) == boost::math::sign(diff1) && std::abs(diff) > std::abs(diff1);
 }
-} // namespace scrimmage
+}  // namespace scrimmage
diff --git a/src/math/Quaternion.cpp b/src/math/Quaternion.cpp
index f02d6a0f67..c4d177e798 100644
--- a/src/math/Quaternion.cpp
+++ b/src/math/Quaternion.cpp
@@ -32,24 +32,31 @@
 
 #include <scrimmage/math/Quaternion.h>
 
-#include <iostream>
 #include <iomanip>
+#include <iostream>
 
 namespace scrimmage {
 
-Quaternion::Quaternion() : Eigen::Quaternion<double, Eigen::DontAlign>(1, 0, 0, 0) {}
+Quaternion::Quaternion() : Eigen::Quaternion<double, Eigen::DontAlign>(1, 0, 0, 0) {
+}
 
-Quaternion::Quaternion(const Quaternion &other) : Eigen::Quaternion<double, Eigen::DontAlign>(other) {}
+Quaternion::Quaternion(const Quaternion& other)
+    : Eigen::Quaternion<double, Eigen::DontAlign>(other) {
+}
 
-Quaternion::Quaternion(const Eigen::Quaternion<double, Eigen::DontAlign> &other) : Eigen::Quaternion<double, Eigen::DontAlign>(other) {}
+Quaternion::Quaternion(const Eigen::Quaternion<double, Eigen::DontAlign>& other)
+    : Eigen::Quaternion<double, Eigen::DontAlign>(other) {
+}
 
-Quaternion::Quaternion(const double &w, const double &x, const double &y, const double &z)
-    : Eigen::Quaternion<double, Eigen::DontAlign>(w, x, y, z) {}
+Quaternion::Quaternion(const double& w, const double& x, const double& y, const double& z)
+    : Eigen::Quaternion<double, Eigen::DontAlign>(w, x, y, z) {
+}
 
-Quaternion::Quaternion(double &w, double &x, double &y, double &z)
-    : Eigen::Quaternion<double, Eigen::DontAlign>(w, x, y, z) {}
+Quaternion::Quaternion(double& w, double& x, double& y, double& z)
+    : Eigen::Quaternion<double, Eigen::DontAlign>(w, x, y, z) {
+}
 
-Quaternion::Quaternion(const Eigen::Vector3d &vector, double angle_radians) {
+Quaternion::Quaternion(const Eigen::Vector3d& vector, double angle_radians) {
     set(vector, angle_radians);
 }
 
@@ -57,14 +64,14 @@ Quaternion::Quaternion(double roll, double pitch, double yaw) {
     set(roll, pitch, yaw);
 }
 
-Quaternion &Quaternion::operator=(const Quaternion &other) {
+Quaternion& Quaternion::operator=(const Quaternion& other) {
     if (this != &other) {
         set(other.w(), other.x(), other.y(), other.z());
     }
     return *this;
 }
 
-Quaternion &Quaternion::operator=(const Eigen::Quaternion<double, Eigen::DontAlign> &other) {
+Quaternion& Quaternion::operator=(const Eigen::Quaternion<double, Eigen::DontAlign>& other) {
     if (this != &other) {
         set(other.w(), other.x(), other.y(), other.z());
     }
@@ -78,7 +85,7 @@ void Quaternion::set(double w_coeff, double x_coeff, double y_coeff, double z_co
     z() = z_coeff;
 }
 
-void Quaternion::set(const Eigen::Vector3d &vector, double angle_radians) {
+void Quaternion::set(const Eigen::Vector3d& vector, double angle_radians) {
     w() = cos(angle_radians / 2.0);
     Eigen::Vector3d unit_vec = vector.normalized() * sin(angle_radians / 2.0);
     x() = unit_vec.x();
@@ -101,8 +108,7 @@ void Quaternion::set(double roll, double pitch, double yaw) {
 }
 
 double Quaternion::roll() const {
-    return atan2(2 * (w() * x() + y() * z()),
-                 1 - 2 * (pow(x(), 2) + pow(y(), 2)));
+    return atan2(2 * (w() * x() + y() * z()), 1 - 2 * (pow(x(), 2) + pow(y(), 2)));
 }
 
 double Quaternion::pitch() const {
@@ -116,20 +122,19 @@ double Quaternion::pitch_safe() const {
 }
 
 double Quaternion::yaw() const {
-    return atan2(2 * (w() * z() + x() * y()),
-                 1 - 2 * (pow(y(), 2) + pow(z(), 2)));
+    return atan2(2 * (w() * z() + x() * y()), 1 - 2 * (pow(y(), 2) + pow(z(), 2)));
 }
 
 double Quaternion::rotation_angle() const {
     return 2 * acos(w());
 }
 
-Eigen::Vector3d Quaternion::rotate(const Eigen::Vector3d &vec) const {
+Eigen::Vector3d Quaternion::rotate(const Eigen::Vector3d& vec) const {
     Eigen::Quaternion<double, Eigen::DontAlign> pure_quat(0, vec.x(), vec.y(), vec.z());
     return (*this * pure_quat * inverse()).vec();
 }
 
-Eigen::Vector3d Quaternion::rotate_reverse(const Eigen::Vector3d &vec) const {
+Eigen::Vector3d Quaternion::rotate_reverse(const Eigen::Vector3d& vec) const {
     Eigen::Quaternion<double, Eigen::DontAlign> pure_quat(0, vec.x(), vec.y(), vec.z());
     return (inverse() * pure_quat * *this).vec();
 }
@@ -142,10 +147,10 @@ std::ostream& operator<<(std::ostream& os, const Quaternion& q) {
     return os;
 }
 
-scrimmage::Quaternion operator*(const scrimmage::Quaternion &p, const scrimmage::Quaternion &q) {
+scrimmage::Quaternion operator*(const scrimmage::Quaternion& p, const scrimmage::Quaternion& q) {
     const Eigen::Quaternion<double, Eigen::DontAlign>& ep = p;
     const Eigen::Quaternion<double, Eigen::DontAlign>& eq = q;
     return scrimmage::Quaternion(ep * eq);
 }
 
-} // namespace scrimmage
+}  // namespace scrimmage
diff --git a/src/math/State.cpp b/src/math/State.cpp
index 94605c3981..9747540b42 100644
--- a/src/math/State.cpp
+++ b/src/math/State.cpp
@@ -31,42 +31,76 @@
  */
 
 #include <scrimmage/common/Utilities.h>
-#include <scrimmage/math/State.h>
 #include <scrimmage/math/Angles.h>
+#include <scrimmage/math/State.h>
 
-#include <iostream>
 #include <iomanip>
+#include <iostream>
 
 namespace scrimmage {
 
-State::State() : pos_(0, 0, 0), vel_(0, 0, 0), ang_vel_(0, 0, 0) {}
+State::State() : pos_(0, 0, 0), vel_(0, 0, 0), ang_vel_(0, 0, 0) {
+}
 
-State::State(Eigen::Vector3d _pos, Eigen::Vector3d _vel,
-             Eigen::Vector3d _ang_vel, Quaternion _quat) :
-    pos_(_pos), vel_(_vel), ang_vel_(_ang_vel), quat_(_quat) {}
+State::State(Eigen::Vector3d _pos, Eigen::Vector3d _vel, Eigen::Vector3d _ang_vel, Quaternion _quat)
+    : pos_(_pos), vel_(_vel), ang_vel_(_ang_vel), quat_(_quat) {
+}
 
-State::~State() {}
+State::~State() {
+}
 
-Eigen::Vector3d &State::pos() {return pos_;}
-Eigen::Vector3d &State::vel() {return vel_;}
-Eigen::Vector3d &State::ang_vel() {return ang_vel_;}
-Quaternion &State::quat() {return quat_;}
-const Eigen::Vector3d &State::pos() const {return pos_;}
-const Eigen::Vector3d &State::vel() const {return vel_;}
-const Eigen::Vector3d &State::ang_vel() const {return ang_vel_;}
-const Quaternion &State::quat() const {return quat_;}
+Eigen::Vector3d& State::pos() {
+    return pos_;
+}
+Eigen::Vector3d& State::vel() {
+    return vel_;
+}
+Eigen::Vector3d& State::ang_vel() {
+    return ang_vel_;
+}
+Quaternion& State::quat() {
+    return quat_;
+}
+const Eigen::Vector3d& State::pos() const {
+    return pos_;
+}
+const Eigen::Vector3d& State::vel() const {
+    return vel_;
+}
+const Eigen::Vector3d& State::ang_vel() const {
+    return ang_vel_;
+}
+const Quaternion& State::quat() const {
+    return quat_;
+}
 
 // for backwards compatibility
-const Eigen::Vector3d &State::pos_const() const {return pos_;}
-const Eigen::Vector3d &State::vel_const() const {return vel_;}
-const Eigen::Vector3d &State::ang_vel_const() const {return ang_vel_;}
-const Quaternion &State::quat_const() const {return quat_;}
-void State::set_pos(const Eigen::Vector3d &pos) {pos_ = pos;}
-void State::set_vel(const Eigen::Vector3d &vel) {vel_ = vel;}
-void State::set_ang_vel(const Eigen::Vector3d &ang_vel) {ang_vel_ = ang_vel;}
-void State::set_quat(const Quaternion &quat) {quat_ = quat;}
-
-bool State::InFieldOfView(State &other, double fov_width, double fov_height) const {
+const Eigen::Vector3d& State::pos_const() const {
+    return pos_;
+}
+const Eigen::Vector3d& State::vel_const() const {
+    return vel_;
+}
+const Eigen::Vector3d& State::ang_vel_const() const {
+    return ang_vel_;
+}
+const Quaternion& State::quat_const() const {
+    return quat_;
+}
+void State::set_pos(const Eigen::Vector3d& pos) {
+    pos_ = pos;
+}
+void State::set_vel(const Eigen::Vector3d& vel) {
+    vel_ = vel;
+}
+void State::set_ang_vel(const Eigen::Vector3d& ang_vel) {
+    ang_vel_ = ang_vel;
+}
+void State::set_quat(const Quaternion& quat) {
+    quat_ = quat;
+}
+
+bool State::InFieldOfView(State& other, double fov_width, double fov_height) const {
     Eigen::Vector3d rel_pos = this->rel_pos_local_frame(other.pos());
     double az = atan2(rel_pos(1), rel_pos(0));
     double norm_xy = sqrt(pow(rel_pos(0), 2) + pow(rel_pos(1), 2));
@@ -74,7 +108,7 @@ bool State::InFieldOfView(State &other, double fov_width, double fov_height) con
     return std::abs(az) < fov_width / 2 && std::abs(el) < fov_height / 2;
 }
 
-Eigen::Vector3d State::rel_pos_local_frame(Eigen::Vector3d &other) const {
+Eigen::Vector3d State::rel_pos_local_frame(Eigen::Vector3d& other) const {
     return quat_.rotate_reverse(other - pos_);
 }
 
@@ -90,7 +124,7 @@ Eigen::Vector3d State::orient_global_frame() const {
     return quat_.rotate(Eigen::Vector3d::UnitX());
 }
 
-double State::rel_az(const Eigen::Vector3d &other) const {
+double State::rel_az(const Eigen::Vector3d& other) const {
     Eigen::Vector3d diff = other - pos_;
     double az = atan2(diff(1), diff(0));
     return Angles::angle_diff_rad(az, quat_.yaw());
@@ -113,16 +147,15 @@ Eigen::Matrix4d State::tf_matrix(bool enable_translate) {
 }
 
 std::ostream& operator<<(std::ostream& os, const State& s) {
-    const Quaternion &q = s.quat();
-
-    os << "(" << eigen_str(s.pos_, s.output_precision)
-        << "), (" << eigen_str(s.vel_, s.output_precision)
-        << "), (" << eigen_str(s.ang_vel_, s.output_precision)
-        << "), ("
-        << std::setprecision(s.output_precision) << q.roll() << ", "
-        << std::setprecision(s.output_precision) << q.pitch() << ", "
-        << std::setprecision(s.output_precision) << q.yaw() << ")";
+    const Quaternion& q = s.quat();
+
+    os << "(" << eigen_str(s.pos_, s.output_precision) << "), ("
+       << eigen_str(s.vel_, s.output_precision) << "), ("
+       << eigen_str(s.ang_vel_, s.output_precision) << "), ("
+       << std::setprecision(s.output_precision) << q.roll() << ", "
+       << std::setprecision(s.output_precision) << q.pitch() << ", "
+       << std::setprecision(s.output_precision) << q.yaw() << ")";
     return os;
 }
 
-} // namespace scrimmage
+}  // namespace scrimmage
diff --git a/src/math/StateWithCovariance.cpp b/src/math/StateWithCovariance.cpp
index f69a3b2222..7fb257d2f7 100644
--- a/src/math/StateWithCovariance.cpp
+++ b/src/math/StateWithCovariance.cpp
@@ -32,28 +32,24 @@
 #include <scrimmage/math/StateWithCovariance.h>
 
 namespace scrimmage {
-StateWithCovariance::StateWithCovariance() :
-    covariance_(Eigen::MatrixXd::Identity(3, 3)) {
+StateWithCovariance::StateWithCovariance() : covariance_(Eigen::MatrixXd::Identity(3, 3)) {
 }
 
-StateWithCovariance::StateWithCovariance(const scrimmage::State &state) :
-    StateWithCovariance(state, 3, 3, 1.0) {
+StateWithCovariance::StateWithCovariance(const scrimmage::State& state)
+    : StateWithCovariance(state, 3, 3, 1.0) {
 }
 
-StateWithCovariance::StateWithCovariance(const scrimmage::State &state,
-                                         const int &cov_num_rows,
-                                         const int &cov_num_cols,
-                                         const double &cov_diag) :
-    State(state.pos(), state.vel(), state.ang_vel(),
-          state.quat()),
-    covariance_(cov_diag * Eigen::MatrixXd::Identity(cov_num_rows, cov_num_cols)) {
+StateWithCovariance::StateWithCovariance(const scrimmage::State& state, const int& cov_num_rows,
+                                         const int& cov_num_cols, const double& cov_diag)
+    : State(state.pos(), state.vel(), state.ang_vel(), state.quat()),
+      covariance_(cov_diag * Eigen::MatrixXd::Identity(cov_num_rows, cov_num_cols)) {
 }
 
-void StateWithCovariance::set_covariance(const Eigen::MatrixXd &covariance) {
+void StateWithCovariance::set_covariance(const Eigen::MatrixXd& covariance) {
     covariance_ = covariance;
 }
 
-const Eigen::MatrixXd &StateWithCovariance::covariance() {
+const Eigen::MatrixXd& StateWithCovariance::covariance() {
     return covariance_;
 }
-} // namespace scrimmage
+}  // namespace scrimmage
diff --git a/src/metrics/Metrics.cpp b/src/metrics/Metrics.cpp
index 8ebae8e82a..03742b7f4f 100644
--- a/src/metrics/Metrics.cpp
+++ b/src/metrics/Metrics.cpp
@@ -37,25 +37,38 @@
 
 namespace scrimmage {
 
-Metrics::Metrics() {}
+Metrics::Metrics() {
+}
 
-Metrics::~Metrics() {}
+Metrics::~Metrics() {
+}
 
-void Metrics::init() {}
+void Metrics::init() {
+}
 
-void Metrics::init(std::map<std::string, std::string> &params) {}
+void Metrics::init(std::map<std::string, std::string>& params) {
+}
 
-bool Metrics::step_metrics(double t, double dt) { return false; }
+bool Metrics::step_metrics(double t, double dt) {
+    return false;
+}
 
-void Metrics::calc_team_scores() {}
+void Metrics::calc_team_scores() {
+}
 
-void Metrics::print_team_summaries() {}
+void Metrics::print_team_summaries() {
+}
 
-std::map<int, std::map<std::string, double> > &Metrics::team_metrics()
-{ return team_metrics_; }
+std::map<int, std::map<std::string, double> >& Metrics::team_metrics() {
+    return team_metrics_;
+}
 
-std::list<std::string> &Metrics::headers() { return headers_; }
+std::list<std::string>& Metrics::headers() {
+    return headers_;
+}
 
-std::map<int, double> &Metrics::team_scores() { return team_scores_; }
+std::map<int, double>& Metrics::team_scores() {
+    return team_scores_;
+}
 
-} // namespace scrimmage
+}  // namespace scrimmage
diff --git a/src/network/Interface.cpp b/src/network/Interface.cpp
index a54ba73230..8a78670d51 100644
--- a/src/network/Interface.cpp
+++ b/src/network/Interface.cpp
@@ -32,8 +32,8 @@
 
 #if ENABLE_GRPC
 #include <grpc++/grpc++.h>
-#include <scrimmage/proto/Scrimmage.grpc.pb.h>
 #include <scrimmage/network/ScrimmageServiceImpl.h>
+#include <scrimmage/proto/Scrimmage.grpc.pb.h>
 using grpc::Channel;
 using grpc::ClientContext;
 using grpc::Status;
@@ -41,12 +41,12 @@ using grpc::Status;
 
 #include <scrimmage/common/Utilities.h>
 #include <scrimmage/entity/Contact.h>
+#include <scrimmage/network/Interface.h>
 #include <scrimmage/proto/ProtoConversions.h>
 #include <scrimmage/proto/Shape.pb.h>
-#include <scrimmage/network/Interface.h>
 
 #include <iostream>
-#include <thread> // NOLINT
+#include <thread>  // NOLINT
 
 using std::cout;
 using std::endl;
@@ -59,7 +59,7 @@ void Interface::start_server() {
 }
 #endif
 
-bool Interface::init_network(Interface::Mode_t mode, const std::string &ip, int port) {
+bool Interface::init_network(Interface::Mode_t mode, const std::string& ip, int port) {
     mode_ = mode;
     ip_ = ip;
     port_ = port;
@@ -88,9 +88,9 @@ bool Interface::init_network(Interface::Mode_t mode, const std::string &ip, int
 #if ENABLE_GRPC
         std::string result = ip_ + ":" + std::to_string(port_);
         std::shared_ptr<Channel> channel(
-            grpc::CreateChannel(result, grpc::InsecureChannelCredentials())
-        );
-        std::unique_ptr<scrimmage_proto::ScrimmageService::Stub> frame_temp(scrimmage_proto::ScrimmageService::NewStub(channel));
+            grpc::CreateChannel(result, grpc::InsecureChannelCredentials()));
+        std::unique_ptr<scrimmage_proto::ScrimmageService::Stub> frame_temp(
+            scrimmage_proto::ScrimmageService::NewStub(channel));
         scrimmage_stub_ = std::move(frame_temp);
         cout << "Client connecting to " << result << endl;
 #else
@@ -113,9 +113,9 @@ bool Interface::check_ready() {
     // the non-connection
     std::string result = ip_ + ":" + std::to_string(port_);
     std::shared_ptr<Channel> channel(
-        grpc::CreateChannel(result, grpc::InsecureChannelCredentials())
-    );
-    std::unique_ptr<scrimmage_proto::ScrimmageService::Stub> stub(scrimmage_proto::ScrimmageService::NewStub(channel));
+        grpc::CreateChannel(result, grpc::InsecureChannelCredentials()));
+    std::unique_ptr<scrimmage_proto::ScrimmageService::Stub> stub(
+        scrimmage_proto::ScrimmageService::NewStub(channel));
 
     google::protobuf::Empty req;
     scrimmage_proto::BlankReply reply;
@@ -126,7 +126,7 @@ bool Interface::check_ready() {
 #endif
 }
 
-bool Interface::send_frame(std::shared_ptr<scrimmage_proto::Frame> &frame) {
+bool Interface::send_frame(std::shared_ptr<scrimmage_proto::Frame>& frame) {
     if (mode_ == shared) {
         push_frame(frame);
     } else if (mode_ == client) {
@@ -159,15 +159,13 @@ bool Interface::send_frame(std::shared_ptr<scrimmage_proto::Frame> &frame) {
     return true;
 }
 
-bool Interface::send_frame(double time,
-                           std::shared_ptr<ContactMap> &contacts) {
-    std::shared_ptr<scrimmage_proto::Frame> frame =
-        create_frame(time, contacts);
+bool Interface::send_frame(double time, std::shared_ptr<ContactMap>& contacts) {
+    std::shared_ptr<scrimmage_proto::Frame> frame = create_frame(time, contacts);
 
     return send_frame(frame);
 }
 
-bool Interface::send_utm_terrain(std::shared_ptr<scrimmage_proto::UTMTerrain> &utm_terrain) {
+bool Interface::send_utm_terrain(std::shared_ptr<scrimmage_proto::UTMTerrain>& utm_terrain) {
     if (caching_enabled_) {
         utm_terrain_cache_ = utm_terrain;
     }
@@ -204,7 +202,7 @@ bool Interface::send_utm_terrain(std::shared_ptr<scrimmage_proto::UTMTerrain> &u
     return true;
 }
 
-bool Interface::send_contact_visual(std::shared_ptr<scrimmage_proto::ContactVisual> &cv) {
+bool Interface::send_contact_visual(std::shared_ptr<scrimmage_proto::ContactVisual>& cv) {
     if (caching_enabled_) {
         contact_visual_cache_.push_back(cv);
     }
@@ -241,7 +239,7 @@ bool Interface::send_contact_visual(std::shared_ptr<scrimmage_proto::ContactVisu
     return true;
 }
 
-bool Interface::send_gui_msg(scrimmage_proto::GUIMsg &gui_msg) {
+bool Interface::send_gui_msg(scrimmage_proto::GUIMsg& gui_msg) {
     if (mode_ == shared) {
         push_gui_msg(gui_msg);
     } else if (mode_ == client) {
@@ -274,7 +272,7 @@ bool Interface::send_gui_msg(scrimmage_proto::GUIMsg &gui_msg) {
     return true;
 }
 
-bool Interface::send_world_point_clicked_msg(scrimmage_proto::WorldPointClicked &msg) {
+bool Interface::send_world_point_clicked_msg(scrimmage_proto::WorldPointClicked& msg) {
     if (mode_ == shared) {
         push_world_point_clicked_msg(msg);
     } else if (mode_ == client) {
@@ -307,8 +305,7 @@ bool Interface::send_world_point_clicked_msg(scrimmage_proto::WorldPointClicked
     return true;
 }
 
-
-bool Interface::send_sim_info(scrimmage_proto::SimInfo &sim_info) {
+bool Interface::send_sim_info(scrimmage_proto::SimInfo& sim_info) {
     if (mode_ == shared) {
         push_sim_info(sim_info);
     } else if (mode_ == client) {
@@ -341,7 +338,7 @@ bool Interface::send_sim_info(scrimmage_proto::SimInfo &sim_info) {
     return true;
 }
 
-bool Interface::send_shapes(scrimmage_proto::Shapes &shapes) {
+bool Interface::send_shapes(scrimmage_proto::Shapes& shapes) {
     if (mode_ == shared) {
         push_shapes(shapes);
     } else if (mode_ == client) {
@@ -391,14 +388,14 @@ void Interface::send_cached() {
     caching_enabled_ = true;
 }
 
-bool Interface::push_contact_visual(std::shared_ptr<scrimmage_proto::ContactVisual> &cv) {
+bool Interface::push_contact_visual(std::shared_ptr<scrimmage_proto::ContactVisual>& cv) {
     contact_visual_mutex.lock();
     contact_visual_list_.push_back(cv);
     contact_visual_mutex.unlock();
     return true;
 }
 
-bool Interface::push_frame(std::shared_ptr<scrimmage_proto::Frame> &frame) {
+bool Interface::push_frame(std::shared_ptr<scrimmage_proto::Frame>& frame) {
     frames_mutex.lock();
     frames_list_.push_back(frame);
     if (frames_list_.size() > max_queue_size_) {
@@ -408,28 +405,28 @@ bool Interface::push_frame(std::shared_ptr<scrimmage_proto::Frame> &frame) {
     return true;
 }
 
-bool Interface::push_utm_terrain(std::shared_ptr<scrimmage_proto::UTMTerrain> &utm_terrain) {
+bool Interface::push_utm_terrain(std::shared_ptr<scrimmage_proto::UTMTerrain>& utm_terrain) {
     utm_terrain_mutex.lock();
     utm_terrain_list_.push_back(utm_terrain);
     utm_terrain_mutex.unlock();
     return true;
 }
 
-bool Interface::push_gui_msg(scrimmage_proto::GUIMsg &gui_msg) {
+bool Interface::push_gui_msg(scrimmage_proto::GUIMsg& gui_msg) {
     gui_msg_mutex.lock();
     gui_msg_list_.push_back(gui_msg);
     gui_msg_mutex.unlock();
     return true;
 }
 
-bool Interface::push_world_point_clicked_msg(scrimmage_proto::WorldPointClicked &msg) {
+bool Interface::push_world_point_clicked_msg(scrimmage_proto::WorldPointClicked& msg) {
     world_point_clicked_msg_mutex.lock();
     world_point_clicked_msg_list_.push_back(msg);
     world_point_clicked_msg_mutex.unlock();
     return true;
 }
 
-bool Interface::push_sim_info(scrimmage_proto::SimInfo &sim_info) {
+bool Interface::push_sim_info(scrimmage_proto::SimInfo& sim_info) {
     sim_info_mutex.lock();
     sim_info_list_.push_back(sim_info);
     if (sim_info_list_.size() > max_queue_size_) {
@@ -439,7 +436,7 @@ bool Interface::push_sim_info(scrimmage_proto::SimInfo &sim_info) {
     return true;
 }
 
-bool Interface::push_shapes(scrimmage_proto::Shapes &shapes) {
+bool Interface::push_shapes(scrimmage_proto::Shapes& shapes) {
     shapes_mutex.lock();
     shapes_list_.push_back(shapes);
 
@@ -499,4 +496,4 @@ bool Interface::shapes_update() {
     return status;
 }
 
-} // namespace scrimmage
+}  // namespace scrimmage
diff --git a/src/network/ScrimmageServiceImpl.cpp b/src/network/ScrimmageServiceImpl.cpp
index 868774d1c7..091fbb7d10 100644
--- a/src/network/ScrimmageServiceImpl.cpp
+++ b/src/network/ScrimmageServiceImpl.cpp
@@ -32,30 +32,41 @@
 
 #if ENABLE_GRPC
 
-#include <scrimmage/network/ScrimmageServiceImpl.h>
 #include <scrimmage/network/Interface.h>
+#include <scrimmage/network/ScrimmageServiceImpl.h>
 
-scrimmage::ScrimmageServiceImpl::ScrimmageServiceImpl(scrimmage::Interface *interface) : interface_(interface) {}
+scrimmage::ScrimmageServiceImpl::ScrimmageServiceImpl(scrimmage::Interface* interface)
+    : interface_(interface) {
+}
 
-grpc::Status scrimmage::ScrimmageServiceImpl::SendFrame(grpc::ServerContext *context, const scrimmage_proto::Frame *frame, scrimmage_proto::BlankReply *reply) {
+grpc::Status scrimmage::ScrimmageServiceImpl::SendFrame(grpc::ServerContext* context,
+                                                        const scrimmage_proto::Frame* frame,
+                                                        scrimmage_proto::BlankReply* reply) {
     std::shared_ptr<scrimmage_proto::Frame> f = std::make_shared<scrimmage_proto::Frame>(*frame);
     interface_->push_frame(f);
     return grpc::Status::OK;
 }
 
-grpc::Status scrimmage::ScrimmageServiceImpl::SendUTMTerrain(grpc::ServerContext *context, const scrimmage_proto::UTMTerrain *terrain, scrimmage_proto::BlankReply *reply) {
-    std::shared_ptr<scrimmage_proto::UTMTerrain> t = std::make_shared<scrimmage_proto::UTMTerrain>(*terrain);
+grpc::Status scrimmage::ScrimmageServiceImpl::SendUTMTerrain(
+    grpc::ServerContext* context, const scrimmage_proto::UTMTerrain* terrain,
+    scrimmage_proto::BlankReply* reply) {
+    std::shared_ptr<scrimmage_proto::UTMTerrain> t =
+        std::make_shared<scrimmage_proto::UTMTerrain>(*terrain);
     interface_->push_utm_terrain(t);
     return grpc::Status::OK;
 }
 
-grpc::Status scrimmage::ScrimmageServiceImpl::SendSimInfo(grpc::ServerContext *context, const scrimmage_proto::SimInfo *sim_info, scrimmage_proto::BlankReply *reply) {
+grpc::Status scrimmage::ScrimmageServiceImpl::SendSimInfo(grpc::ServerContext* context,
+                                                          const scrimmage_proto::SimInfo* sim_info,
+                                                          scrimmage_proto::BlankReply* reply) {
     scrimmage_proto::SimInfo si = *sim_info;
     interface_->push_sim_info(si);
     return grpc::Status::OK;
 }
 
-grpc::Status scrimmage::ScrimmageServiceImpl::SendGUIMsg(grpc::ServerContext *context, const scrimmage_proto::GUIMsg *gui_msg, scrimmage_proto::BlankReply *reply) {
+grpc::Status scrimmage::ScrimmageServiceImpl::SendGUIMsg(grpc::ServerContext* context,
+                                                         const scrimmage_proto::GUIMsg* gui_msg,
+                                                         scrimmage_proto::BlankReply* reply) {
     scrimmage_proto::GUIMsg si = *gui_msg;
     interface_->push_gui_msg(si);
     if (si.shutting_down()) {
@@ -64,13 +75,18 @@ grpc::Status scrimmage::ScrimmageServiceImpl::SendGUIMsg(grpc::ServerContext *co
     return grpc::Status::OK;
 }
 
-grpc::Status scrimmage::ScrimmageServiceImpl::SendContactVisual(grpc::ServerContext *context, const scrimmage_proto::ContactVisual *contact_visual, scrimmage_proto::BlankReply *reply) {
-    std::shared_ptr<scrimmage_proto::ContactVisual> cv = std::make_shared<scrimmage_proto::ContactVisual>(*contact_visual);
+grpc::Status scrimmage::ScrimmageServiceImpl::SendContactVisual(
+    grpc::ServerContext* context, const scrimmage_proto::ContactVisual* contact_visual,
+    scrimmage_proto::BlankReply* reply) {
+    std::shared_ptr<scrimmage_proto::ContactVisual> cv =
+        std::make_shared<scrimmage_proto::ContactVisual>(*contact_visual);
     interface_->push_contact_visual(cv);
     return grpc::Status::OK;
 }
 
-grpc::Status scrimmage::ScrimmageServiceImpl::SendShapes(grpc::ServerContext *context, const scrimmage_proto::Shapes *shape, scrimmage_proto::BlankReply *reply) {
+grpc::Status scrimmage::ScrimmageServiceImpl::SendShapes(grpc::ServerContext* context,
+                                                         const scrimmage_proto::Shapes* shape,
+                                                         scrimmage_proto::BlankReply* reply) {
     scrimmage_proto::Shapes s = *shape;
     interface_->push_shapes(s);
     return grpc::Status::OK;
@@ -82,4 +98,4 @@ grpc::Status scrimmage::ScrimmageServiceImpl::Ready(grpc::ServerContext* context
     return grpc::Status::OK;
 }
 
-#endif // ENABLE_GRPC
+#endif  // ENABLE_GRPC
diff --git a/src/parse/ConfigParse.cpp b/src/parse/ConfigParse.cpp
index cfa839ddf8..3c5a42f5a1 100644
--- a/src/parse/ConfigParse.cpp
+++ b/src/parse/ConfigParse.cpp
@@ -30,12 +30,12 @@
  *
  */
 
-#include <scrimmage/common/Utilities.h>
 #include <scrimmage/common/FileSearch.h>
+#include <scrimmage/common/Utilities.h>
 #include <scrimmage/parse/ConfigParse.h>
 
-#include <iostream>
 #include <fstream>
+#include <iostream>
 #include <vector>
 
 #define BOOST_NO_CXX11_SCOPED_ENUMS
@@ -43,7 +43,6 @@
 #include <boost/filesystem/path.hpp>
 #undef BOOST_NO_CXX11_SCOPED_ENUMS
 #include <boost/algorithm/string.hpp>
-
 #include <rapidxml/rapidxml.hpp>
 
 namespace fs = boost::filesystem;
@@ -54,19 +53,19 @@ using std::endl;
 
 namespace scrimmage {
 
-ConfigParse::ConfigParse() {}
+ConfigParse::ConfigParse() {
+}
 
 void ConfigParse::set_required(std::string node_name) {
     required_.push_back(node_name);
 }
 
-void ConfigParse::recursive_params(rx::xml_node<char> *root,
-        const std::map<std::string, std::string> &overrides,
-        std::map<std::string, std::string> &params,
-        const std::string &prev) {
+void ConfigParse::recursive_params(rx::xml_node<char>* root,
+                                   const std::map<std::string, std::string>& overrides,
+                                   std::map<std::string, std::string>& params,
+                                   const std::string& prev) {
     // End condition
-    if (root == 0 || root->name() == std::string("")
-        || root->name() == std::string(" ")) {
+    if (root == 0 || root->name() == std::string("") || root->name() == std::string(" ")) {
         return;
     }
 
@@ -85,7 +84,7 @@ void ConfigParse::recursive_params(rx::xml_node<char> *root,
             // It does exist, grab the current size and increment.
             size = std::stoi(params[name_size]) + 1;
         }
-        name = name + "_" + std::to_string(size-1);
+        name = name + "_" + std::to_string(size - 1);
         params[name_size] = std::to_string(size);
     }
 
@@ -102,10 +101,8 @@ void ConfigParse::recursive_params(rx::xml_node<char> *root,
     recursive_params(root->next_sibling(), overrides, params, prev);
 }
 
-bool ConfigParse::parse(const std::map<std::string, std::string> &overrides,
-        std::string filename, std::string env_var,
-        FileSearch &file_search, bool verbose) {
-
+bool ConfigParse::parse(const std::map<std::string, std::string>& overrides, std::string filename,
+                        std::string env_var, FileSearch& file_search, bool verbose) {
     std::string result = "";
     bool status = file_search.find_file(filename, "xml", env_var, result, verbose);
     if (!status) {
@@ -128,7 +125,7 @@ bool ConfigParse::parse(const std::map<std::string, std::string> &overrides,
     std::string content(buffer.str());
     doc.parse<0>(&content[0]);
 
-    rx::xml_node<> *config_node = doc.first_node("params");
+    rx::xml_node<>* config_node = doc.first_node("params");
     if (config_node == 0) {
         cout << "Missing tag: params" << endl;
         return false;
@@ -143,13 +140,13 @@ bool ConfigParse::parse(const std::map<std::string, std::string> &overrides,
     // Determine if there were any overrides (XML attributes) specified in the
     // mission file that weren't declared in the Plugin's XML
     // file. Automatically add these overrides to the params block.
-    for (auto &kv : overrides) {
+    for (auto& kv : overrides) {
         if (params_.count(kv.first) == 0) {
             params_[kv.first] = kv.second;
         }
     }
 
-    for (std::string &node_name : required_) {
+    for (std::string& node_name : required_) {
         if (params_.count(node_name) == 0) {
             cout << "Config file is missing XML tag: " << node_name << endl;
             return false;
@@ -158,9 +155,13 @@ bool ConfigParse::parse(const std::map<std::string, std::string> &overrides,
     return true;
 }
 
-std::map<std::string, std::string> &ConfigParse::params() { return params_; }
+std::map<std::string, std::string>& ConfigParse::params() {
+    return params_;
+}
 
-std::string ConfigParse::filename() { return filename_; }
+std::string ConfigParse::filename() {
+    return filename_;
+}
 
 std::string ConfigParse::directory() {
     if (fs::exists(filename_)) {
@@ -184,15 +185,15 @@ std::string ConfigParse::stem() {
 }
 
 void ConfigParse::print_params() {
-    for (auto &kv : params_) {
+    for (auto& kv : params_) {
         cout << kv.first << "=" << kv.second << endl;
     }
 }
 
 std::ostream& operator<<(std::ostream& os, ConfigParse& cp) {
-    for (auto &kv : cp.params()) {
+    for (auto& kv : cp.params()) {
         os << kv.first << "=" << kv.second << endl;
     }
     return os;
 }
-} // namespace scrimmage
+}  // namespace scrimmage
diff --git a/src/parse/MissionParse.cpp b/src/parse/MissionParse.cpp
index cf17f2f24f..726527b10d 100644
--- a/src/parse/MissionParse.cpp
+++ b/src/parse/MissionParse.cpp
@@ -43,17 +43,16 @@
 #include <scrimmage/parse/XMLParser/LibXML2Parser.h>
 #endif
 
+#include <GeographicLib/Geocentric.hpp>
+#include <GeographicLib/LocalCartesian.hpp>
+#include <GeographicLib/UTMUPS.hpp>
+#include <boost/algorithm/string.hpp>
 #include <fstream>
 #include <iostream>
 #include <regex>  //NOLINT
 #include <string>
 #include <typeinfo>
 
-#include <GeographicLib/Geocentric.hpp>
-#include <GeographicLib/LocalCartesian.hpp>
-#include <GeographicLib/UTMUPS.hpp>
-#include <boost/algorithm/string.hpp>
-
 #define BOOST_NO_CXX11_SCOPED_ENUMS
 #include <boost/filesystem.hpp>
 #undef BOOST_NO_CXX11_SCOPED_ENUMS
@@ -64,13 +63,13 @@ using std::endl;
 namespace fs = boost::filesystem;
 namespace scrimmage {
 
-void MissionParse::set_overrides(const std::string &overrides) {
+void MissionParse::set_overrides(const std::string& overrides) {
     std::string overrides_no_space = remove_whitespace(overrides);
 
     // Iterate over comma-separated overrides.
     std::vector<std::string> overrides_tokens;
     split(overrides_tokens, overrides_no_space, ", ");
-    for (auto &overrides_str : overrides_tokens) {
+    for (auto& overrides_str : overrides_tokens) {
         // Parse each key=value pair
         std::vector<std::string> kv_tokens;
         split(kv_tokens, overrides_str, ":= ");
@@ -82,7 +81,7 @@ void MissionParse::set_overrides(const std::string &overrides) {
     }
 }
 
-bool MissionParse::parse(const std::string &filename) {
+bool MissionParse::parse(const std::string& filename) {
     read_file_content(filename);
     mission_file_content_ = replace_overrides(mission_file_content_);
 #if ENABLE_LIBXML2_PARSER
@@ -98,7 +97,7 @@ bool MissionParse::parse(const std::string &filename) {
 #endif
 }
 
-bool MissionParse::read_file_content(const std::string &filename) {
+bool MissionParse::read_file_content(const std::string& filename) {
     mission_filename_ = expand_user(filename);
     // First, explicitly search for the mission file.
     if (!fs::exists(mission_filename_)) {
@@ -106,8 +105,8 @@ bool MissionParse::read_file_content(const std::string &filename) {
         // SCRIMMAGE_MISSION_PATH.
         FileSearch file_search;
         std::string result = "";
-        bool status = file_search.find_file(
-            mission_filename_, "xml", "SCRIMMAGE_MISSION_PATH", result, false);
+        bool status = file_search.find_file(mission_filename_, "xml", "SCRIMMAGE_MISSION_PATH",
+                                            result, false);
         if (!status) {
             // The mission file wasn't found. Exit.
             cout << "SCRIMMAGE mission file not found: " << mission_filename_ << endl;
@@ -133,7 +132,7 @@ bool MissionParse::read_file_content(const std::string &filename) {
 std::string MissionParse::replace_overrides(std::string str) {
     // Search and replace any overrides of the form ${key=value} in the mission
     // file
-    for (auto &kv : overrides_map_) {
+    for (auto& kv : overrides_map_) {
         std::regex reg("\\$\\{" + kv.first + "=(.+?)\\}");
         str = std::regex_replace(str, reg, kv.second);
     }
@@ -160,21 +159,21 @@ bool MissionParse::parse_filecontents(Parser& doc) {
 #if ENABLE_LIBXML2_PARSER
 template <>
 bool MissionParse::parse_filecontents<LibXML2Parser>(LibXML2Parser& doc) {
-    // LibXMLParser is used when other xml files are included via xinclude to create a composite 
-    // xml mission file. We need to parse this contents twice to ensure that proper override substitution
-    // occurs
+    // LibXMLParser is used when other xml files are included via xinclude to create a composite
+    // xml mission file. We need to parse this contents twice to ensure that proper override
+    // substitution occurs
     doc.set_filename(mission_filename_);
     std::vector<char> mission_file_content_vec(mission_file_content_.size());
     mission_file_content_vec.assign(mission_file_content_.begin(),
                                     mission_file_content_.end());  // copy
-    doc.parse(mission_file_content_vec); 
+    doc.parse(mission_file_content_vec);
     mission_file_content_vec = doc.get_filecontents();
 
-    mission_file_content_.assign(mission_file_content_vec.cbegin(), mission_file_content_vec.cend());
+    mission_file_content_.assign(mission_file_content_vec.cbegin(),
+                                 mission_file_content_vec.cend());
     mission_file_content_ = replace_overrides(mission_file_content_);
-    mission_file_content_vec.assign(mission_file_content_.begin(),
-                                    mission_file_content_.end());
-    
+    mission_file_content_vec.assign(mission_file_content_.begin(), mission_file_content_.end());
+
     return doc.parse(mission_file_content_vec);
 }
 #endif
@@ -190,13 +189,13 @@ bool MissionParse::parse_mission() {
     auto parse_double = [&](std::string str) { return std::stod(replace_overrides(str)); };
 
     Parser doc;
-    //doc.set_filename(mission_filename_);
+    // doc.set_filename(mission_filename_);
     //// doc.parse requires a null terminated string that it can modify.
-    //std::vector<char> mission_file_content_vec(mission_file_content_.size() + 1);
-    //mission_file_content_vec.assign(mission_file_content_.begin(),
-    //                                mission_file_content_.end());  // copy
-    //mission_file_content_vec.push_back('\0');                      // shouldn't reallocate
-    //doc.parse(mission_file_content_vec);
+    // std::vector<char> mission_file_content_vec(mission_file_content_.size() + 1);
+    // mission_file_content_vec.assign(mission_file_content_.begin(),
+    //                                 mission_file_content_.end());  // copy
+    // mission_file_content_vec.push_back('\0');                      // shouldn't reallocate
+    // doc.parse(mission_file_content_vec);
 
     parse_filecontents(doc);
     auto runscript_node = doc.first_node("runscript");
@@ -239,7 +238,7 @@ bool MissionParse::parse_mission() {
         }
     }
 
-    auto parse_tags = [&](const std::string &tagname, std::list<std::string> &tag_list) {
+    auto parse_tags = [&](const std::string& tagname, std::list<std::string>& tag_list) {
         for (auto node = runscript_node.first_node(tagname); node.is_valid();
              node = node.next_sibling(tagname)) {
             // If a "name" is specified, use this name
@@ -292,7 +291,7 @@ bool MissionParse::parse_mission() {
             for (auto attr = node.first_attribute(); attr.is_valid(); attr = attr.next()) {
                 std::string attr_name = attr.name();
                 if (attr_name == "param_common") {
-                    for (auto &kv : param_common[attr.value()]) {
+                    for (auto& kv : param_common[attr.value()]) {
                         attributes_[nm2][kv.first] = kv.second;
                         attributes_[nm3][kv.first] = kv.second;
                         attributes_[nm4][kv.first] = kv.second;
@@ -353,7 +352,7 @@ bool MissionParse::parse_mission() {
     // Create a directory to hold the log data
     // Use the current time for the directory's name
     time_t rawtime;
-    struct tm *timeinfo;
+    struct tm* timeinfo;
     char time_buffer[80];
     time(&rawtime);
     timeinfo = localtime(&rawtime);
@@ -409,7 +408,7 @@ bool MissionParse::parse_mission() {
             for (auto attr = node.first_attribute(); attr.is_valid(); attr = attr.next()) {
                 const std::string attr_name = attr.name();
                 if (attr_name == "param_common") {
-                    for (auto &kv : param_common[attr.value()]) {
+                    for (auto& kv : param_common[attr.value()]) {
                         entity_common_attributes[nm][node_name][kv.first] = kv.second;
                     }
                 } else {
@@ -534,8 +533,8 @@ bool MissionParse::parse_mission() {
 
             // If lon/lat/alt are defined, overwrite x/y/z
             if (lon_valid && lat_valid && alt_valid) {
-                proj_->Forward(
-                    pos_LLA(1), pos_LLA(0), pos_LLA(2), pos_xyz(0), pos_xyz(1), pos_xyz(2));
+                proj_->Forward(pos_LLA(1), pos_LLA(0), pos_LLA(2), pos_xyz(0), pos_xyz(1),
+                               pos_xyz(2));
             }
             team_info_[team_id].bases.push_back(pos_xyz);
             team_info_[team_id].radii.push_back(radius);
@@ -562,7 +561,7 @@ bool MissionParse::parse_mission() {
             for (auto attr = node.first_attribute(); attr.is_valid(); attr = attr.next()) {
                 const std::string attr_name = attr.name();
                 if (attr_name == "param_common") {
-                    for (auto &kv : param_common[attr.value()]) {
+                    for (auto& kv : param_common[attr.value()]) {
                         entity_attributes_[ent_desc_id][nm][kv.first] = kv.second;
                     }
                 } else {
@@ -721,8 +720,8 @@ bool MissionParse::parse_mission() {
 
 bool MissionParse::create_log_dir() {
     // Create the root_log_dir_ if it doesn't exist:
-    if (not fs::exists(fs::path(root_log_dir_)) &&
-        not fs::create_directories(fs::path(root_log_dir_))) {
+    if (not fs::exists(fs::path(root_log_dir_))
+        && not fs::create_directories(fs::path(root_log_dir_))) {
         cout << "Failed to create the root_log_dir: " << root_log_dir_ << endl;
         return false;
     }
@@ -766,8 +765,8 @@ bool MissionParse::create_log_dir() {
     // Create the latest log directory by default. Don't create the latest
     // directory if the tag is defined in the mission file and it is set to
     // false.
-    bool create_latest_dir = not(params_.count("create_latest_dir") > 0 &&
-                                 str2bool(params_["create_latest_dir"]) == false);
+    bool create_latest_dir = not(params_.count("create_latest_dir") > 0
+                                 && str2bool(params_["create_latest_dir"]) == false);
 
     if (create_latest_dir) {
         boost::system::error_code ec;
@@ -801,25 +800,47 @@ bool MissionParse::create_log_dir() {
     return true;
 }
 
-bool MissionParse::write(const std::string &file) { return true; }
+bool MissionParse::write(const std::string& file) {
+    return true;
+}
 
-double MissionParse::t0() { return t0_; }
+double MissionParse::t0() {
+    return t0_;
+}
 
-double MissionParse::tend() { return tend_; }
+double MissionParse::tend() {
+    return tend_;
+}
 
-double MissionParse::dt() { return dt_; }
+double MissionParse::dt() {
+    return dt_;
+}
 
-void MissionParse::set_dt(const double &dt) { dt_ = dt; }
+void MissionParse::set_dt(const double& dt) {
+    dt_ = dt;
+}
 
-double MissionParse::motion_multiplier() { return motion_multiplier_; }
+double MissionParse::motion_multiplier() {
+    return motion_multiplier_;
+}
 
-double MissionParse::time_warp() { return time_warp_; }
+double MissionParse::time_warp() {
+    return time_warp_;
+}
 
-bool MissionParse::start_paused() { return start_paused_; }
+bool MissionParse::start_paused() {
+    return start_paused_;
+}
 
-const bool &MissionParse::full_screen() { return full_screen_; }
-const unsigned &MissionParse::window_width() { return window_width_; }
-const unsigned &MissionParse::window_height() { return window_height_; }
+const bool& MissionParse::full_screen() {
+    return full_screen_;
+}
+const unsigned& MissionParse::window_width() {
+    return window_width_;
+}
+const unsigned& MissionParse::window_height() {
+    return window_height_;
+}
 
 bool MissionParse::parse_terrain() {
     ConfigParse terrain_parse;
@@ -857,33 +878,27 @@ bool MissionParse::parse_terrain() {
     utm_terrain_->set_enable_terrain(false);
     utm_terrain_->set_enable_extrusion(false);
 
-    if (params_.count("terrain") > 0 &&
-        find_terrain_files(params_["terrain"], terrain_parse, utm_terrain_)) {
+    if (params_.count("terrain") > 0
+        && find_terrain_files(params_["terrain"], terrain_parse, utm_terrain_)) {
         double x_easting;
         double y_northing;
         int zone;
         bool northp;
         double gamma, k_scale;
-        GeographicLib::UTMUPS::Forward(latitude_origin(),
-                                       longitude_origin(),
-                                       zone,
-                                       northp,
-                                       x_easting,
-                                       y_northing,
-                                       gamma,
-                                       k_scale);
+        GeographicLib::UTMUPS::Forward(latitude_origin(), longitude_origin(), zone, northp,
+                                       x_easting, y_northing, gamma, k_scale);
 
         // Make sure the projected zone and hemisphere match the input xml
-        if (((northp && boost::to_upper_copy(terrain_parse.params()["hemisphere"]) == "NORTH") ||
-             (!northp && boost::to_upper_copy(terrain_parse.params()["hemisphere"]) == "SOUTH")) &&
-            zone == get("zone", terrain_parse.params(), -2)) {
+        if (((northp && boost::to_upper_copy(terrain_parse.params()["hemisphere"]) == "NORTH")
+             || (!northp && boost::to_upper_copy(terrain_parse.params()["hemisphere"]) == "SOUTH"))
+            && zone == get("zone", terrain_parse.params(), -2)) {
             double origin_offset_x = 0;
             double origin_offset_y = 0;
             double origin_offset_z = 0;
 
-            if (terrain_parse.params().count("origin_offset_x") == 1 &&
-                terrain_parse.params().count("origin_offset_y") == 1 &&
-                terrain_parse.params().count("origin_offset_z") == 1) {
+            if (terrain_parse.params().count("origin_offset_x") == 1
+                && terrain_parse.params().count("origin_offset_y") == 1
+                && terrain_parse.params().count("origin_offset_z") == 1) {
                 origin_offset_x = stod(terrain_parse.params()["origin_offset_x"]);
                 origin_offset_y = stod(terrain_parse.params()["origin_offset_y"]);
                 origin_offset_z = stod(terrain_parse.params()["origin_offset_z"]);
@@ -917,42 +932,72 @@ bool MissionParse::parse_terrain() {
     return false;
 }
 
-scrimmage_proto::Color &MissionParse::background_color() { return background_color_; }
+scrimmage_proto::Color& MissionParse::background_color() {
+    return background_color_;
+}
 
-std::string MissionParse::log_dir() { return log_dir_; }
-std::string MissionParse::root_log_dir() { return root_log_dir_; }
+std::string MissionParse::log_dir() {
+    return log_dir_;
+}
+std::string MissionParse::root_log_dir() {
+    return root_log_dir_;
+}
 
-void MissionParse::set_log_dir(const std::string &log_dir) { log_dir_ = log_dir; }
+void MissionParse::set_log_dir(const std::string& log_dir) {
+    log_dir_ = log_dir;
+}
 
-std::map<int, AttributeMap> &MissionParse::entity_attributes() { return entity_attributes_; }
+std::map<int, AttributeMap>& MissionParse::entity_attributes() {
+    return entity_attributes_;
+}
 
-std::map<int, std::map<std::string, std::string>> &MissionParse::entity_params() {
+std::map<int, std::map<std::string, std::string>>& MissionParse::entity_params() {
     return entity_params_;
 }
 
-std::map<int, int> &MissionParse::ent_id_to_block_id() { return ent_id_to_block_id_; }
+std::map<int, int>& MissionParse::ent_id_to_block_id() {
+    return ent_id_to_block_id_;
+}
 
-EntityDesc_t &MissionParse::entity_descriptions() { return entity_descs_; }
+EntityDesc_t& MissionParse::entity_descriptions() {
+    return entity_descs_;
+}
 
-std::map<std::string, int> &MissionParse::entity_tag_to_id() { return entity_tag_to_id_; }
+std::map<std::string, int>& MissionParse::entity_tag_to_id() {
+    return entity_tag_to_id_;
+}
 
-bool MissionParse::enable_gui() { return enable_gui_; }
+bool MissionParse::enable_gui() {
+    return enable_gui_;
+}
 
-bool MissionParse::network_gui() { return network_gui_; }
+bool MissionParse::network_gui() {
+    return network_gui_;
+}
 
-AttributeMap &MissionParse::attributes() { return attributes_; }
+AttributeMap& MissionParse::attributes() {
+    return attributes_;
+}
 
-std::map<std::string, std::string> &MissionParse::params() { return params_; }
+std::map<std::string, std::string>& MissionParse::params() {
+    return params_;
+}
 
-double MissionParse::longitude_origin() { return longitude_origin_; }
+double MissionParse::longitude_origin() {
+    return longitude_origin_;
+}
 
-double MissionParse::latitude_origin() { return latitude_origin_; }
+double MissionParse::latitude_origin() {
+    return latitude_origin_;
+}
 
-double MissionParse::altitude_origin() { return altitude_origin_; }
+double MissionParse::altitude_origin() {
+    return altitude_origin_;
+}
 
-void MissionParse::set_lat_lon_alt_origin(const double &latitude_origin,
-                                          const double &longitude_origin,
-                                          const double &altitude_origin) {
+void MissionParse::set_lat_lon_alt_origin(const double& latitude_origin,
+                                          const double& longitude_origin,
+                                          const double& altitude_origin) {
     latitude_origin_ = latitude_origin;
     longitude_origin_ = longitude_origin;
     altitude_origin_ = altitude_origin;
@@ -961,44 +1006,76 @@ void MissionParse::set_lat_lon_alt_origin(const double &latitude_origin,
         latitude_origin_, longitude_origin_, altitude_origin_, GeographicLib::Geocentric::WGS84());
 }
 
-std::map<int, TeamInfo> &MissionParse::team_info() { return team_info_; }
+std::map<int, TeamInfo>& MissionParse::team_info() {
+    return team_info_;
+}
 
-void MissionParse::set_task_number(int task_num) { task_number_ = task_num; }
+void MissionParse::set_task_number(int task_num) {
+    task_number_ = task_num;
+}
 
-void MissionParse::set_job_number(int job_num) { job_number_ = job_num; }
+void MissionParse::set_job_number(int job_num) {
+    job_number_ = job_num;
+}
 
-std::list<std::string> MissionParse::entity_interactions() { return entity_interactions_; }
+std::list<std::string> MissionParse::entity_interactions() {
+    return entity_interactions_;
+}
 
-std::list<std::string> &MissionParse::network_names() { return network_names_; }
+std::list<std::string>& MissionParse::network_names() {
+    return network_names_;
+}
 
-std::list<std::string> MissionParse::metrics() { return metrics_; }
+std::list<std::string> MissionParse::metrics() {
+    return metrics_;
+}
 
-std::map<int, GenerateInfo> &MissionParse::gen_info() { return gen_info_; }
+std::map<int, GenerateInfo>& MissionParse::gen_info() {
+    return gen_info_;
+}
 
-std::map<int, std::vector<double>> &MissionParse::next_gen_times() { return next_gen_times_; }
+std::map<int, std::vector<double>>& MissionParse::next_gen_times() {
+    return next_gen_times_;
+}
 
-std::shared_ptr<GeographicLib::LocalCartesian> MissionParse::projection() { return proj_; }
+std::shared_ptr<GeographicLib::LocalCartesian> MissionParse::projection() {
+    return proj_;
+}
 
-std::shared_ptr<scrimmage_proto::UTMTerrain> &MissionParse::utm_terrain() { return utm_terrain_; }
+std::shared_ptr<scrimmage_proto::UTMTerrain>& MissionParse::utm_terrain() {
+    return utm_terrain_;
+}
 
-std::string MissionParse::get_mission_filename() { return mission_filename_; }
+std::string MissionParse::get_mission_filename() {
+    return mission_filename_;
+}
 
-bool MissionParse::get_no_bin_logging() { return no_bin_logging_; }
+bool MissionParse::get_no_bin_logging() {
+    return no_bin_logging_;
+}
 
-void MissionParse::set_enable_gui(bool enable) { enable_gui_ = enable; }
+void MissionParse::set_enable_gui(bool enable) {
+    enable_gui_ = enable;
+}
 
-void MissionParse::set_time_warp(double warp) { time_warp_ = warp; }
+void MissionParse::set_time_warp(double warp) {
+    time_warp_ = warp;
+}
 
-void MissionParse::set_network_gui(bool enable) { network_gui_ = enable; }
+void MissionParse::set_network_gui(bool enable) {
+    network_gui_ = enable;
+}
 
-void MissionParse::set_start_paused(bool paused) { start_paused_ = paused; }
+void MissionParse::set_start_paused(bool paused) {
+    start_paused_ = paused;
+}
 
 bool MissionParse::output_required() {
     // If the output_types set is empty, no output is required
     return output_types_.size() != static_cast<unsigned int>(0);
 }
 
-bool MissionParse::output_type_required(const std::string &output_type) {
+bool MissionParse::output_type_required(const std::string& output_type) {
     return output_types_.find(output_type) != output_types_.end();
 }
 
diff --git a/src/parse/ParseUtils.cpp b/src/parse/ParseUtils.cpp
index 359a87b6b9..353a62cc12 100644
--- a/src/parse/ParseUtils.cpp
+++ b/src/parse/ParseUtils.cpp
@@ -33,12 +33,12 @@
 #include <scrimmage/common/FileSearch.h>
 #include <scrimmage/common/PID.h>
 #include <scrimmage/math/Angles.h>
-#include <scrimmage/parse/ParseUtils.h>
 #include <scrimmage/parse/ConfigParse.h>
+#include <scrimmage/parse/ParseUtils.h>
 #include <scrimmage/proto/Visual.pb.h>
 
-#include <iostream>
 #include <fstream>
+#include <iostream>
 #include <sstream>
 #include <vector>
 
@@ -47,8 +47,8 @@
 #include <boost/filesystem/path.hpp>
 #undef BOOST_NO_CXX11_SCOPED_ENUMS
 #include <boost/algorithm/string/case_conv.hpp>
-#include <boost/algorithm/string/split.hpp>
 #include <boost/algorithm/string/classification.hpp>
+#include <boost/algorithm/string/split.hpp>
 
 namespace fs = boost::filesystem;
 using std::cout;
@@ -58,9 +58,8 @@ namespace sc = scrimmage;
 
 namespace scrimmage {
 
-bool find_terrain_files(std::string terrain_name,
-                        ConfigParse &terrain_parse,
-                        std::shared_ptr<scrimmage_proto::UTMTerrain> &utm_terrain) {
+bool find_terrain_files(std::string terrain_name, ConfigParse& terrain_parse,
+                        std::shared_ptr<scrimmage_proto::UTMTerrain>& utm_terrain) {
     if (utm_terrain == nullptr) {
         cout << "utm_terrain is null." << endl;
         return false;
@@ -76,20 +75,23 @@ bool find_terrain_files(std::string terrain_name,
     terrain_parse.set_required("zone");
 
     FileSearch file_search;
-    std::map<std::string, std::string> overrides; // empty, no overrides
+    std::map<std::string, std::string> overrides;  // empty, no overrides
     if (terrain_parse.parse(overrides, terrain_name, "SCRIMMAGE_DATA_PATH", file_search)) {
-        std::string polydata_file = terrain_parse.directory() + "/" + terrain_parse.params()["polydata"];
-        std::string texture_file = terrain_parse.directory() + "/" + terrain_parse.params()["texture"];
-        std::string extrusion_file = terrain_parse.directory() + "/" + terrain_parse.params()["extrusion"];
+        std::string polydata_file =
+            terrain_parse.directory() + "/" + terrain_parse.params()["polydata"];
+        std::string texture_file =
+            terrain_parse.directory() + "/" + terrain_parse.params()["texture"];
+        std::string extrusion_file =
+            terrain_parse.directory() + "/" + terrain_parse.params()["extrusion"];
         std::string extrusion_property = terrain_parse.params()["extrusion_property"];
-        if (fs::exists(extrusion_file) && fs::is_regular_file(extrusion_file) &&
-                !extrusion_property.empty()) {
+        if (fs::exists(extrusion_file) && fs::is_regular_file(extrusion_file)
+            && !extrusion_property.empty()) {
             utm_terrain->set_extrusion_file(extrusion_file);
             utm_terrain->set_extrusion_property(extrusion_property);
             utm_terrain->set_enable_extrusion(true);
         }
-        if (fs::exists(polydata_file) && fs::exists(texture_file) &&
-            fs::is_regular_file(polydata_file) && fs::is_regular_file(texture_file)) {
+        if (fs::exists(polydata_file) && fs::exists(texture_file)
+            && fs::is_regular_file(polydata_file) && fs::is_regular_file(texture_file)) {
             utm_terrain->set_texture_file(texture_file);
             utm_terrain->set_poly_data_file(polydata_file);
             return true;
@@ -98,12 +100,10 @@ bool find_terrain_files(std::string terrain_name,
     return false;
 }
 
-bool find_model_properties(std::string model_name,
-                           ConfigParse &cv_parse,
-                           FileSearch &file_search,
-                           std::map<std::string, std::string> &overrides,
-                           std::shared_ptr<scrimmage_proto::ContactVisual> &cv,
-                           bool &mesh_found, bool &texture_found) {
+bool find_model_properties(std::string model_name, ConfigParse& cv_parse, FileSearch& file_search,
+                           std::map<std::string, std::string>& overrides,
+                           std::shared_ptr<scrimmage_proto::ContactVisual>& cv, bool& mesh_found,
+                           bool& texture_found) {
     if (cv == nullptr) {
         cout << "Contact Visual is null." << endl;
         return false;
@@ -157,8 +157,8 @@ bool find_model_properties(std::string model_name,
     return (mesh_found && texture_found);
 }
 
-bool parse_autonomy_data(std::map<std::string, std::string> &params,
-                         std::map<std::string, std::string> &data_params) {
+bool parse_autonomy_data(std::map<std::string, std::string>& params,
+                         std::map<std::string, std::string>& data_params) {
     if (params.count("data") == 0) {
         cout << "Missing data tag in params" << endl;
         return false;
@@ -166,9 +166,8 @@ bool parse_autonomy_data(std::map<std::string, std::string> &params,
 
     ConfigParse data;
     FileSearch file_search;
-    std::map<std::string, std::string> overrides; // empty, no overrides
-    if (data.parse(overrides, params["data"], "SCRIMMAGE_DATA_PATH",
-                   file_search)) {
+    std::map<std::string, std::string> overrides;  // empty, no overrides
+    if (data.parse(overrides, params["data"], "SCRIMMAGE_DATA_PATH", file_search)) {
         data_params = data.params();
     } else {
         cout << "Failed to find data file: " << params["data"] << endl;
@@ -181,12 +180,10 @@ std::string expand_user(std::string path) {
     if (not path.empty() and path[0] == '~') {
         assert(path.size() == 1 or path[1] == '/');  // or other error handling
         char const* home = std::getenv("HOME");
-        if (home or
-            (home = std::getenv("USERPROFILE"))) {
+        if (home or (home = std::getenv("USERPROFILE"))) {
             path.replace(0, 1, home);
         } else {
-            char const *hdrive = std::getenv("HOMEDRIVE"),
-                *hpath = std::getenv("HOMEPATH");
+            char const *hdrive = std::getenv("HOMEDRIVE"), *hpath = std::getenv("HOMEPATH");
             assert(hdrive);  // or other error handling
             assert(hpath);
             path.replace(0, 1, std::string(hdrive) + hpath);
@@ -197,8 +194,7 @@ std::string expand_user(std::string path) {
 
 bool str2bool(std::string str) {
     // Remove spaces
-    str.erase(std::remove_if(str.begin(), str.end(), ::isspace),
-              str.end());
+    str.erase(std::remove_if(str.begin(), str.end(), ::isspace), str.end());
 
     if (boost::to_upper_copy(str) == "TRUE") {
         return true;
@@ -212,13 +208,12 @@ bool str2bool(std::string str) {
     return (num == 1);
 }
 
-bool get_vec(const std::string &str,
-             std::map<std::string, std::string> & params,
-             std::vector<std::string> &vec) {
+bool get_vec(const std::string& str, std::map<std::string, std::string>& params,
+             std::vector<std::string>& vec) {
     vec.clear();
 
     // Determine if the string name is a list
-    int count = get(str+":size", params, 0);
+    int count = get(str + ":size", params, 0);
     if (count <= 0) return false;
 
     for (int i = 0; i < count; i++) {
@@ -235,7 +230,7 @@ bool get_vec(const std::string &str,
     return true;
 }
 
-Eigen::Vector3d vec2eigen(std::vector<double> &vec) {
+Eigen::Vector3d vec2eigen(std::vector<double>& vec) {
     Eigen::Vector3d v(0, 0, 0);
     if (vec.size() <= 3) {
         for (unsigned int i = 0; i < vec.size(); i++) {
@@ -245,19 +240,16 @@ Eigen::Vector3d vec2eigen(std::vector<double> &vec) {
     return v;
 }
 
-std::string remove_whitespace(const std::string &str) {
+std::string remove_whitespace(const std::string& str) {
     std::string result = str;
-    result.erase(
-        std::remove_if(result.begin(),
-                       result.end(),
-                       [](unsigned char x){return std::isspace(x);}),
-        result.end());
+    result.erase(std::remove_if(result.begin(), result.end(),
+                                [](unsigned char x) { return std::isspace(x); }),
+                 result.end());
     return result;
 }
 
-bool get_vec_of_vecs(const std::string &str,
-                     std::vector<std::vector<std::string>> &out,
-                     const std::string &delims) {
+bool get_vec_of_vecs(const std::string& str, std::vector<std::vector<std::string>>& out,
+                     const std::string& delims) {
     // Of the form...
     // [a b c d]
     // [e f g h]
@@ -270,8 +262,7 @@ bool get_vec_of_vecs(const std::string &str,
 
     // First split based on user defined delimiters (typically ", ")
     std::vector<std::string> tokens;
-    boost::algorithm::split(tokens, str_copy, boost::is_any_of(delims),
-                            boost::token_compress_on);
+    boost::algorithm::split(tokens, str_copy, boost::is_any_of(delims), boost::token_compress_on);
 
     // The tokens vector consists of the delimited items. Break up each vector
     // based on the start "[" and stop "]" delimiters. However, we will allow
@@ -280,13 +271,10 @@ bool get_vec_of_vecs(const std::string &str,
     int open_bracket_count = 0;
     std::string item = "";
     for (std::string token : tokens) {
-
         // Remove whitespace
-        token.erase(
-            std::remove_if(token.begin(),
-                           token.end(),
-                           [](unsigned char x){return std::isspace(x);}),
-            token.end());
+        token.erase(std::remove_if(token.begin(), token.end(),
+                                   [](unsigned char x) { return std::isspace(x); }),
+                    token.end());
 
         // cout << token << endl;
 
@@ -326,26 +314,23 @@ bool get_vec_of_vecs(const std::string &str,
     }
 
     // Remove any empty strings from the individual vectors
-    for (auto &vec : out) {
-        vec.erase(std::remove_if(vec.begin(), vec.end(), [] (auto &str) {
-            return str.empty();
-        }), vec.end());
+    for (auto& vec : out) {
+        vec.erase(std::remove_if(vec.begin(), vec.end(), [](auto& str) { return str.empty(); }),
+                  vec.end());
     }
 
     // Remove any empty vectors from the out vector of vectors
-    out.erase(std::remove_if(out.begin(), out.end(), [] (auto &vec) {
-        return vec.empty();
-    }), out.end());
+    out.erase(std::remove_if(out.begin(), out.end(), [](auto& vec) { return vec.empty(); }),
+              out.end());
 
     return true;
 }
 
-void split(std::vector<std::string> &tokens, const std::string &str,
-           const std::string &delims) {
+void split(std::vector<std::string>& tokens, const std::string& str, const std::string& delims) {
     boost::split(tokens, str, boost::is_any_of(delims));
 }
 
-bool set_pid_gains(sc::PID &pid, std::string str, bool is_angle) {
+bool set_pid_gains(sc::PID& pid, std::string str, bool is_angle) {
     std::vector<std::string> str_vals;
     boost::split(str_vals, str, boost::is_any_of(","));
 
@@ -369,9 +354,8 @@ bool set_pid_gains(sc::PID &pid, std::string str, bool is_angle) {
     return true;
 }
 
-unsigned int parse_plugin_vector(const std::string& key,
-                                 std::map<std::string, std::string> &params,
-                                 std::list<PluginOverrides> &plugin_overrides_list) {
+unsigned int parse_plugin_vector(const std::string& key, std::map<std::string, std::string>& params,
+                                 std::list<PluginOverrides>& plugin_overrides_list) {
     // Parse the behavior plugins
     std::string plugins_str = sc::get<std::string>(key, params, "");
     std::vector<std::vector<std::string>> vecs_of_vecs;
@@ -399,11 +383,8 @@ unsigned int parse_plugin_vector(const std::string& key,
                 if (tokens.size() == 2) {
                     // Remove the quotes from the value
                     tokens[1].erase(
-                        std::remove_if(tokens[1].begin(),
-                                       tokens[1].end(),
-                                       [](unsigned char x){
-                                           return (x == '\"') || (x == '\'');
-                                       }),
+                        std::remove_if(tokens[1].begin(), tokens[1].end(),
+                                       [](unsigned char x) { return (x == '\"') || (x == '\''); }),
                         tokens[1].end());
                     plugin_overrides[tokens[0]] = tokens[1];
                 }
@@ -423,20 +404,16 @@ unsigned int parse_plugin_vector(const std::string& key,
     return plugin_overrides_list.size();
 }
 
-void remove_leading_spaces(std::string &s) {
+void remove_leading_spaces(std::string& s) {
     auto it = std::find_if(s.begin(), s.end(),
-                           [](char c) {
-                               return !std::isspace<char>(c, std::locale::classic());
-                           });
-	s.erase(s.begin(), it);
+                           [](char c) { return !std::isspace<char>(c, std::locale::classic()); });
+    s.erase(s.begin(), it);
 }
 
-void remove_trailing_spaces(std::string &s) {
+void remove_trailing_spaces(std::string& s) {
     auto it = std::find_if(s.rbegin(), s.rend(),
-                           [](char c) {
-                               return !std::isspace<char>(c, std::locale::classic());
-                           });
-	s.erase(it.base(), s.end());
+                           [](char c) { return !std::isspace<char>(c, std::locale::classic()); });
+    s.erase(it.base(), s.end());
 }
 
-} // namespace scrimmage
+}  // namespace scrimmage
diff --git a/src/parse/XMLParser/LibXML2Parser.cpp b/src/parse/XMLParser/LibXML2Parser.cpp
index 781914ae23..66fc3c03c5 100644
--- a/src/parse/XMLParser/LibXML2Parser.cpp
+++ b/src/parse/XMLParser/LibXML2Parser.cpp
@@ -30,11 +30,10 @@
  * similar to LibXML2 interface
  */
 
-#include <scrimmage/parse/XMLParser/LibXML2Parser.h>
-
 #include <libxml/tree.h>
 #include <libxml/xinclude.h>
 #include <libxml/xmlreader.h>
+#include <scrimmage/parse/XMLParser/LibXML2Parser.h>
 
 #include <cassert>
 #include <fstream>
@@ -50,187 +49,187 @@
 namespace fs = boost::filesystem;
 
 namespace scrimmage {
-LibXML2ParserAttribute::LibXML2ParserAttribute(xmlAttrPtr attribute)
-    : attribute_(attribute) {}
-
-LibXML2ParserAttribute LibXML2ParserAttribute::next_attribute(const std::string &name) const {
-  xmlAttrPtr cur_attr = nullptr;
-  const xmlChar *search_name = reinterpret_cast<const xmlChar *>(name.data());
-  for (cur_attr = attribute_->next; cur_attr != nullptr; cur_attr = cur_attr->next) {
-    if (cur_attr->type == XML_ATTRIBUTE_NODE && xmlStrEqual(search_name, cur_attr->name)) {
-      break;
+LibXML2ParserAttribute::LibXML2ParserAttribute(xmlAttrPtr attribute) : attribute_(attribute) {
+}
+
+LibXML2ParserAttribute LibXML2ParserAttribute::next_attribute(const std::string& name) const {
+    xmlAttrPtr cur_attr = nullptr;
+    const xmlChar* search_name = reinterpret_cast<const xmlChar*>(name.data());
+    for (cur_attr = attribute_->next; cur_attr != nullptr; cur_attr = cur_attr->next) {
+        if (cur_attr->type == XML_ATTRIBUTE_NODE && xmlStrEqual(search_name, cur_attr->name)) {
+            break;
+        }
     }
-  }
-  return LibXML2ParserAttribute{cur_attr};
+    return LibXML2ParserAttribute{cur_attr};
 }
 
 LibXML2ParserAttribute LibXML2ParserAttribute::next_attribute() const {
-  return LibXML2ParserAttribute{attribute_->next};
+    return LibXML2ParserAttribute{attribute_->next};
 }
 
 std::string LibXML2ParserAttribute::attribute_name() const {
-  return std::string{reinterpret_cast<const char *>(attribute_->name)};
+    return std::string{reinterpret_cast<const char*>(attribute_->name)};
 }
 
 std::string LibXML2ParserAttribute::attribute_value() const {
-  return std::string{reinterpret_cast<const char *>(
-      xmlNodeListGetString(attribute_->doc, attribute_->children, 1))};
+    return std::string{reinterpret_cast<const char*>(
+        xmlNodeListGetString(attribute_->doc, attribute_->children, 1))};
 }
 
-bool LibXML2ParserAttribute::is_valid_attribute() const { return attribute_ != nullptr; }
+bool LibXML2ParserAttribute::is_valid_attribute() const {
+    return attribute_ != nullptr;
+}
 
-LibXML2ParserNode::LibXML2ParserNode(xmlNodePtr node)
-    : node_(node) {}
+LibXML2ParserNode::LibXML2ParserNode(xmlNodePtr node) : node_(node) {
+}
 
-LibXML2ParserNode LibXML2ParserNode::get_first_node(const std::string &name) const {
-  xmlNode *cur_node = nullptr;
-  const xmlChar *search_name = reinterpret_cast<const xmlChar *>(name.data());
-  if (is_valid_node()) {
-    for (cur_node = node_->children; cur_node != nullptr; cur_node = cur_node->next) {
-      if (cur_node->type == XML_ELEMENT_NODE && xmlStrEqual(search_name, cur_node->name)) {
-        break;
-      }
+LibXML2ParserNode LibXML2ParserNode::get_first_node(const std::string& name) const {
+    xmlNode* cur_node = nullptr;
+    const xmlChar* search_name = reinterpret_cast<const xmlChar*>(name.data());
+    if (is_valid_node()) {
+        for (cur_node = node_->children; cur_node != nullptr; cur_node = cur_node->next) {
+            if (cur_node->type == XML_ELEMENT_NODE && xmlStrEqual(search_name, cur_node->name)) {
+                break;
+            }
+        }
     }
-  }
-  return LibXML2ParserNode{cur_node};
+    return LibXML2ParserNode{cur_node};
 }
 
 LibXML2ParserNode LibXML2ParserNode::get_first_node() const {
-  xmlNode *cur_node = nullptr;
-  if (is_valid_node()) {
-    for (cur_node = node_->children; cur_node != nullptr; cur_node = cur_node->next) {
-      if (cur_node->type == XML_ELEMENT_NODE) {
-        break;
-      }
+    xmlNode* cur_node = nullptr;
+    if (is_valid_node()) {
+        for (cur_node = node_->children; cur_node != nullptr; cur_node = cur_node->next) {
+            if (cur_node->type == XML_ELEMENT_NODE) {
+                break;
+            }
+        }
     }
-  }
-  return LibXML2ParserNode{cur_node};
+    return LibXML2ParserNode{cur_node};
 }
 
-LibXML2ParserNode LibXML2ParserNode::get_next_sibling(const std::string &name) const {
-  xmlNodePtr cur_node = nullptr;
-  const xmlChar *search_name = reinterpret_cast<const xmlChar *>(name.data());
-  for (cur_node = node_->next; cur_node != nullptr; cur_node = cur_node->next) {
-    if (cur_node->type == XML_ELEMENT_NODE && xmlStrEqual(search_name, cur_node->name)) {
-      break;
+LibXML2ParserNode LibXML2ParserNode::get_next_sibling(const std::string& name) const {
+    xmlNodePtr cur_node = nullptr;
+    const xmlChar* search_name = reinterpret_cast<const xmlChar*>(name.data());
+    for (cur_node = node_->next; cur_node != nullptr; cur_node = cur_node->next) {
+        if (cur_node->type == XML_ELEMENT_NODE && xmlStrEqual(search_name, cur_node->name)) {
+            break;
+        }
     }
-  }
-  return LibXML2ParserNode{cur_node};
+    return LibXML2ParserNode{cur_node};
 }
 
 LibXML2ParserNode LibXML2ParserNode::get_next_sibling() const {
-  xmlNodePtr cur_node = nullptr;
-  for (cur_node = node_->next; cur_node != nullptr; cur_node = cur_node->next) {
-    if (cur_node->type == XML_ELEMENT_NODE) {
-      break;
+    xmlNodePtr cur_node = nullptr;
+    for (cur_node = node_->next; cur_node != nullptr; cur_node = cur_node->next) {
+        if (cur_node->type == XML_ELEMENT_NODE) {
+            break;
+        }
     }
-  }
-  return LibXML2ParserNode{cur_node};
+    return LibXML2ParserNode{cur_node};
 }
 
-LibXML2ParserAttribute LibXML2ParserNode::get_first_attribute(const std::string &name) const {
-  xmlAttrPtr cur_attr = nullptr;
-  const xmlChar *search_name = reinterpret_cast<const xmlChar *>(name.data());
-  for (cur_attr = node_->properties; cur_attr != nullptr; cur_attr = cur_attr->next) {
-    if (cur_attr->type == XML_ATTRIBUTE_NODE && xmlStrEqual(search_name, cur_attr->name)) {
-      break;
+LibXML2ParserAttribute LibXML2ParserNode::get_first_attribute(const std::string& name) const {
+    xmlAttrPtr cur_attr = nullptr;
+    const xmlChar* search_name = reinterpret_cast<const xmlChar*>(name.data());
+    for (cur_attr = node_->properties; cur_attr != nullptr; cur_attr = cur_attr->next) {
+        if (cur_attr->type == XML_ATTRIBUTE_NODE && xmlStrEqual(search_name, cur_attr->name)) {
+            break;
+        }
     }
-  }
-  return LibXML2ParserAttribute{cur_attr};
+    return LibXML2ParserAttribute{cur_attr};
 }
 
 LibXML2ParserAttribute LibXML2ParserNode::get_first_attribute() const {
-  return LibXML2ParserAttribute{node_->properties};
+    return LibXML2ParserAttribute{node_->properties};
 }
 
 std::string LibXML2ParserNode::node_name() const {
-  return std::string{reinterpret_cast<const char *>(node_->name)};
+    return std::string{reinterpret_cast<const char*>(node_->name)};
 }
 
 std::string LibXML2ParserNode::node_value() const {
-  char *value = reinterpret_cast<char *>(xmlNodeGetContent(node_));
-  std::string value_str = std::string{(value != nullptr) ? value : ""};
-  xmlFree(value);
-  return value_str;
+    char* value = reinterpret_cast<char*>(xmlNodeGetContent(node_));
+    std::string value_str = std::string{(value != nullptr) ? value : ""};
+    xmlFree(value);
+    return value_str;
 }
 
-bool LibXML2ParserNode::is_valid_node() const { return node_ != nullptr; }
+bool LibXML2ParserNode::is_valid_node() const {
+    return node_ != nullptr;
+}
 
 LibXML2ParserDocument::~LibXML2ParserDocument() {
-  xmlFreeDoc(doc_);
-  xmlCleanupParser();
-}
-
-bool LibXML2ParserDocument::parse_document(const std::string &filename) {
-  fs::path path{filename};
-  if (fs::is_symlink(path)) {
-    path = fs::read_symlink(path);
-  }
-  assert(fs::exists(path) && fs::is_regular_file(path));
-  std::ifstream file(path.string());
-  std::stringstream buffer;
-  buffer << file.rdbuf();
-  file.close();
-  std::string filecontent_str = buffer.str();
-  filecontents_.assign(filecontent_str.cbegin(), filecontent_str.cend());
-  filename_ = path.string();
-  return parse_document(filecontents_);
-}
-
-bool LibXML2ParserDocument::parse_document(std::vector<char> &filecontent) {
-  LIBXML_TEST_VERSION;
-  //doc_ = xmlReadFile(filename_.c_str(), NULL, LibXML2ParserDocument::PARSING_OPTIONS);
-  // Filecontents can not be null terminated
-  if(filecontent.back() == '\0') {
-    filecontent.pop_back();
-  }
-  filecontents_ = filecontent;
-  doc_ = xmlReadMemory(filecontents_.data(), filecontents_.size(), filename_.c_str(), NULL, LibXML2ParserDocument::PARSING_OPTIONS);
-  if (doc_ != nullptr) {
-    int ret = xmlXIncludeProcess(doc_);
-    return ret == 0;
-  } else {
-    return false;
-  }
+    xmlFreeDoc(doc_);
+    xmlCleanupParser();
+}
+
+bool LibXML2ParserDocument::parse_document(const std::string& filename) {
+    fs::path path{filename};
+    if (fs::is_symlink(path)) {
+        path = fs::read_symlink(path);
+    }
+    assert(fs::exists(path) && fs::is_regular_file(path));
+    std::ifstream file(path.string());
+    std::stringstream buffer;
+    buffer << file.rdbuf();
+    file.close();
+    std::string filecontent_str = buffer.str();
+    filecontents_.assign(filecontent_str.cbegin(), filecontent_str.cend());
+    filename_ = path.string();
+    return parse_document(filecontents_);
+}
+
+bool LibXML2ParserDocument::parse_document(std::vector<char>& filecontent) {
+    LIBXML_TEST_VERSION;
+    // doc_ = xmlReadFile(filename_.c_str(), NULL, LibXML2ParserDocument::PARSING_OPTIONS);
+    //  Filecontents can not be null terminated
+    if (filecontent.back() == '\0') {
+        filecontent.pop_back();
+    }
+    filecontents_ = filecontent;
+    doc_ = xmlReadMemory(filecontents_.data(), filecontents_.size(), filename_.c_str(), NULL,
+                         LibXML2ParserDocument::PARSING_OPTIONS);
+    if (doc_ != nullptr) {
+        int ret = xmlXIncludeProcess(doc_);
+        return ret == 0;
+    } else {
+        return false;
+    }
 }
 
 const std::vector<char>& LibXML2ParserDocument::get_filecontents() {
     if (doc_ != nullptr) {
         xmlChar* buffer;
         int size;
-        xmlDocDumpMemoryEnc(doc_,
-                &buffer,
-                &size,
-                "UTF-8");
+        xmlDocDumpMemoryEnc(doc_, &buffer, &size, "UTF-8");
 
         filecontents_.assign(&buffer[0], &buffer[size]);
         xmlFree(buffer);
     }
-    
+
     return filecontents_;
 }
 
-
-
-LibXML2ParserNode LibXML2ParserDocument::find_first_node(const std::string &name) const {
-  xmlNodePtr cur_node = nullptr;
-  const xmlChar *search_name = reinterpret_cast<const xmlChar *>(name.data());
-  for (cur_node = doc_->children; cur_node != nullptr; cur_node = cur_node->next) {
-    if (cur_node->type == XML_ELEMENT_NODE && xmlStrEqual(search_name, cur_node->name)) {
-      break;
+LibXML2ParserNode LibXML2ParserDocument::find_first_node(const std::string& name) const {
+    xmlNodePtr cur_node = nullptr;
+    const xmlChar* search_name = reinterpret_cast<const xmlChar*>(name.data());
+    for (cur_node = doc_->children; cur_node != nullptr; cur_node = cur_node->next) {
+        if (cur_node->type == XML_ELEMENT_NODE && xmlStrEqual(search_name, cur_node->name)) {
+            break;
+        }
     }
-  }
-  return LibXML2ParserNode{cur_node};
+    return LibXML2ParserNode{cur_node};
 }
 
 LibXML2ParserNode LibXML2ParserDocument::find_first_node() const {
-  xmlNodePtr cur_node = nullptr;
-  for (cur_node = doc_->children; cur_node != nullptr; cur_node = cur_node->next) {
-    if (cur_node->type == XML_ELEMENT_NODE) {
-      break;
+    xmlNodePtr cur_node = nullptr;
+    for (cur_node = doc_->children; cur_node != nullptr; cur_node = cur_node->next) {
+        if (cur_node->type == XML_ELEMENT_NODE) {
+            break;
+        }
     }
-  }
-  return LibXML2ParserNode{cur_node};
+    return LibXML2ParserNode{cur_node};
 }
 
 }  // namespace scrimmage
diff --git a/src/parse/XMLParser/RapidXMLParser.cpp b/src/parse/XMLParser/RapidXMLParser.cpp
index 31e7cce21b..6d860ce509 100644
--- a/src/parse/XMLParser/RapidXMLParser.cpp
+++ b/src/parse/XMLParser/RapidXMLParser.cpp
@@ -34,12 +34,11 @@
 
 #include <fstream>
 #include <iostream>
+#include <rapidxml/rapidxml.hpp>
 #include <sstream>
 #include <string>
 #include <vector>
 
-#include <rapidxml/rapidxml.hpp>
-
 #define BOOST_NO_CXX11_SCOPED_ENUMS
 #include <boost/filesystem.hpp>
 #undef BOOST_NO_CXX11_SCOPED_ENUMS
@@ -48,114 +47,119 @@ namespace fs = boost::filesystem;
 
 namespace scrimmage {
 
-RapidXMLParserAttribute::RapidXMLParserAttribute(rapidxml::xml_attribute<> *attribute)
-    : attribute_(attribute) {}
+RapidXMLParserAttribute::RapidXMLParserAttribute(rapidxml::xml_attribute<>* attribute)
+    : attribute_(attribute) {
+}
 
-RapidXMLParserAttribute RapidXMLParserAttribute::next_attribute(const std::string &name) const {
-  return RapidXMLParserAttribute{attribute_->next_attribute(name.c_str())};
+RapidXMLParserAttribute RapidXMLParserAttribute::next_attribute(const std::string& name) const {
+    return RapidXMLParserAttribute{attribute_->next_attribute(name.c_str())};
 }
 
 RapidXMLParserAttribute RapidXMLParserAttribute::next_attribute() const {
-  return RapidXMLParserAttribute{attribute_->next_attribute()};
+    return RapidXMLParserAttribute{attribute_->next_attribute()};
 }
 
 std::string RapidXMLParserAttribute::attribute_name() const {
-  if (!this->is_valid() || attribute_->value_size() == 0) {
-    return "";
-  }
-  return std::string{attribute_->name(), attribute_->name_size()};
+    if (!this->is_valid() || attribute_->value_size() == 0) {
+        return "";
+    }
+    return std::string{attribute_->name(), attribute_->name_size()};
 }
 
 std::string RapidXMLParserAttribute::attribute_value() const {
-  // Values may not always be null-terminated, depending of parsing settings
-  if (!this->is_valid() || attribute_->value_size() == 0) {
-    return "";
-  }
-  return std::string{attribute_->value(), attribute_->value_size()};
+    // Values may not always be null-terminated, depending of parsing settings
+    if (!this->is_valid() || attribute_->value_size() == 0) {
+        return "";
+    }
+    return std::string{attribute_->value(), attribute_->value_size()};
 }
 
-bool RapidXMLParserAttribute::is_valid_attribute() const { return attribute_ != nullptr; }
+bool RapidXMLParserAttribute::is_valid_attribute() const {
+    return attribute_ != nullptr;
+}
 
-RapidXMLParserNode::RapidXMLParserNode(rapidxml::xml_node<> *node)
-    : node_(node) {}
+RapidXMLParserNode::RapidXMLParserNode(rapidxml::xml_node<>* node) : node_(node) {
+}
 
-RapidXMLParserNode RapidXMLParserNode::get_first_node(const std::string &name) const {
-  return RapidXMLParserNode{node_->first_node(name.c_str())};
+RapidXMLParserNode RapidXMLParserNode::get_first_node(const std::string& name) const {
+    return RapidXMLParserNode{node_->first_node(name.c_str())};
 }
 
 RapidXMLParserNode RapidXMLParserNode::get_first_node() const {
-  return RapidXMLParserNode{node_->first_node()};
+    return RapidXMLParserNode{node_->first_node()};
 }
 
-RapidXMLParserNode RapidXMLParserNode::get_next_sibling(const std::string &name) const {
-  return RapidXMLParserNode{node_->next_sibling(name.c_str())};
+RapidXMLParserNode RapidXMLParserNode::get_next_sibling(const std::string& name) const {
+    return RapidXMLParserNode{node_->next_sibling(name.c_str())};
 }
 
 RapidXMLParserNode RapidXMLParserNode::get_next_sibling() const {
-  return RapidXMLParserNode{node_->next_sibling()};
+    return RapidXMLParserNode{node_->next_sibling()};
 }
 
-RapidXMLParserAttribute RapidXMLParserNode::get_first_attribute(const std::string &name) const {
-  return RapidXMLParserAttribute{node_->first_attribute(name.c_str())};
+RapidXMLParserAttribute RapidXMLParserNode::get_first_attribute(const std::string& name) const {
+    return RapidXMLParserAttribute{node_->first_attribute(name.c_str())};
 }
 
 RapidXMLParserAttribute RapidXMLParserNode::get_first_attribute() const {
-  return RapidXMLParserAttribute{node_->first_attribute()};
+    return RapidXMLParserAttribute{node_->first_attribute()};
 }
 
 std::string RapidXMLParserNode::node_name() const {
-  if (!this->is_valid() || node_->name_size() == 0) {
-    return std::string{""};
-  }
-  return std::string{node_->name(), node_->name_size()};
+    if (!this->is_valid() || node_->name_size() == 0) {
+        return std::string{""};
+    }
+    return std::string{node_->name(), node_->name_size()};
 }
 
 std::string RapidXMLParserNode::node_value() const {
-  if (!this->is_valid() || node_->value_size() == 0) {
-    return std::string{""};
-  }
-  return std::string{node_->value(), node_->value_size()};
-}
-
-bool RapidXMLParserNode::is_valid_node() const { return node_ != nullptr; }
-
-bool RapidXMLParserDocument::parse_document(const std::string &filename) {
-  fs::path path{filename};
-  if (fs::is_symlink(path)) {
-    path = fs::read_symlink(path);
-  }
-  assert(fs::exists(path) && fs::is_regular_file(path));
-  std::ifstream file(path.string());
-  std::stringstream buffer;
-  buffer << file.rdbuf();
-  file.close();
-  std::string filecontent_str = buffer.str();
-  std::vector<char> filecontent(filecontent_str.cbegin(), filecontent_str.cend());
-  filename_ = path.string();
-  return parse_document(filecontent);
-}
-
-bool RapidXMLParserDocument::parse_document(const std::vector<char> &filecontent) {
-  filecontent_ = filecontent;
-  try {
-    // Note: This parse function can hard fail (seg fault, no exception) on
-    //       badly formatted xml data. Sometimes it'll except, sometimes not.
-    doc_.parse<rapidxml::parse_non_destructive>(filecontent_.data());
-  } catch (...) {
-    std::cerr << "scrimmage::MissionParse::parse: Exception during "
-                 "rapidxml::xml_document<>.parse<>()."
-                 "Your xml mission file may be ill-formatted\n";
-    return false;
-  }
-  return true;
-}
-
-RapidXMLParserNode RapidXMLParserDocument::find_first_node(const std::string &name) const {
-  return RapidXMLParserNode{doc_.first_node(name.c_str())};
+    if (!this->is_valid() || node_->value_size() == 0) {
+        return std::string{""};
+    }
+    return std::string{node_->value(), node_->value_size()};
+}
+
+bool RapidXMLParserNode::is_valid_node() const {
+    return node_ != nullptr;
+}
+
+bool RapidXMLParserDocument::parse_document(const std::string& filename) {
+    fs::path path{filename};
+    if (fs::is_symlink(path)) {
+        path = fs::read_symlink(path);
+    }
+    assert(fs::exists(path) && fs::is_regular_file(path));
+    std::ifstream file(path.string());
+    std::stringstream buffer;
+    buffer << file.rdbuf();
+    file.close();
+    std::string filecontent_str = buffer.str();
+    std::vector<char> filecontent(filecontent_str.cbegin(), filecontent_str.cend());
+    filename_ = path.string();
+    return parse_document(filecontent);
+}
+
+bool RapidXMLParserDocument::parse_document(const std::vector<char>& filecontent) {
+    filecontent_ = filecontent;
+    try {
+        // Note: This parse function can hard fail (seg fault, no exception) on
+        //       badly formatted xml data. Sometimes it'll except, sometimes not.
+        doc_.parse<rapidxml::parse_non_destructive>(filecontent_.data());
+    } catch (...) {
+        std::cerr << "scrimmage::MissionParse::parse: Exception during "
+                     "rapidxml::xml_document<>.parse<>()."
+                     "Your xml mission file may be ill-formatted\n";
+        return false;
+    }
+    return true;
+}
+
+RapidXMLParserNode RapidXMLParserDocument::find_first_node(const std::string& name) const {
+    return RapidXMLParserNode{doc_.first_node(name.c_str())};
 }
 
 RapidXMLParserNode RapidXMLParserDocument::find_first_node() const {
-  return RapidXMLParserNode{doc_.first_node()};
+    return RapidXMLParserNode{doc_.first_node()};
 }
 
 }  // namespace scrimmage
diff --git a/src/plugin_manager/MotionModel.cpp b/src/plugin_manager/MotionModel.cpp
index 7710ec9467..e69d5646cc 100644
--- a/src/plugin_manager/MotionModel.cpp
+++ b/src/plugin_manager/MotionModel.cpp
@@ -31,30 +31,45 @@
  */
 
 #include <scrimmage/motion/MotionModel.h>
-#include <functional>
+
 #include <boost/numeric/odeint.hpp>
+#include <functional>
 
 namespace pl = std::placeholders;
 
 namespace scrimmage {
 
-MotionModel::MotionModel() : ext_force_(0, 0, 0), ext_moment_(0, 0, 0),
-                             mass_(1.0), g_(9.81) {}
+MotionModel::MotionModel() : ext_force_(0, 0, 0), ext_moment_(0, 0, 0), mass_(1.0), g_(9.81) {
+}
 
-std::string MotionModel::type() { return std::string("MotionModel"); }
+std::string MotionModel::type() {
+    return std::string("MotionModel");
+}
 
-bool MotionModel::init(std::map<std::string, std::string> &info, std::map<std::string, std::string> &params)
-{ return false; }
+bool MotionModel::init(std::map<std::string, std::string>& info,
+                       std::map<std::string, std::string>& params) {
+    return false;
+}
 
-bool MotionModel::step(double time, double dt) { return true; }
+bool MotionModel::step(double time, double dt) {
+    return true;
+}
 
-bool MotionModel::posthumous(double t) { return true; }
+bool MotionModel::posthumous(double t) {
+    return true;
+}
 
-StatePtr &MotionModel::state() {return state_;}
+StatePtr& MotionModel::state() {
+    return state_;
+}
 
-void MotionModel::set_state(StatePtr &state) {state_ = state;}
+void MotionModel::set_state(StatePtr& state) {
+    state_ = state;
+}
 
-void MotionModel::teleport(StatePtr &state) {state_ = state;}
+void MotionModel::teleport(StatePtr& state) {
+    state_ = state;
+}
 
 void MotionModel::ode_step(double dt) {
     auto sys = std::bind(&MotionModel::model, this, pl::_1, pl::_2, pl::_3);
@@ -62,17 +77,18 @@ void MotionModel::ode_step(double dt) {
     stepper.do_step(sys, x_, 0, dt);
 }
 
-void MotionModel::model(const MotionModel::vector_t &x, MotionModel::vector_t &dxdt, double t) {}
+void MotionModel::model(const MotionModel::vector_t& x, MotionModel::vector_t& dxdt, double t) {
+}
 
-void MotionModel::set_external_force(const Eigen::Vector3d &force) {
+void MotionModel::set_external_force(const Eigen::Vector3d& force) {
     ext_force_ = force;
 }
 
-void MotionModel::set_external_moment(const Eigen::Vector3d &moment) {
+void MotionModel::set_external_moment(const Eigen::Vector3d& moment) {
     ext_moment_ = moment;
 }
 
 void MotionModel::close(double t) {
     state_ = nullptr;
 }
-} // namespace scrimmage
+}  // namespace scrimmage
diff --git a/src/plugin_manager/Plugin.cpp b/src/plugin_manager/Plugin.cpp
index abf912c633..31d80f4016 100644
--- a/src/plugin_manager/Plugin.cpp
+++ b/src/plugin_manager/Plugin.cpp
@@ -32,14 +32,20 @@
 
 #include <scrimmage/entity/EntityPlugin.h>
 
-#include <string>
 #include <memory>
+#include <string>
 
 namespace scrimmage {
-Plugin::Plugin() : name_("Plugin") {}
-Plugin::~Plugin() {}
-std::string Plugin::name() { return name_; }
-std::string Plugin::type() { return std::string("Plugin"); }
+Plugin::Plugin() : name_("Plugin") {
+}
+Plugin::~Plugin() {
+}
+std::string Plugin::name() {
+    return name_;
+}
+std::string Plugin::type() {
+    return std::string("Plugin");
+}
 
 void Plugin::set_name(std::string name) {
     if (name_set_ == true) {
@@ -50,4 +56,4 @@ void Plugin::set_name(std::string name) {
     name_set_ = true;  // Set only once before warning.
 }
 
-} // namespace scrimmage
+}  // namespace scrimmage
diff --git a/src/plugin_manager/PluginManager.cpp b/src/plugin_manager/PluginManager.cpp
index 763fd5472a..1287f96e07 100644
--- a/src/plugin_manager/PluginManager.cpp
+++ b/src/plugin_manager/PluginManager.cpp
@@ -31,23 +31,21 @@
  */
 
 #include <dlfcn.h>
-
-#include <scrimmage/common/Utilities.h>
 #include <scrimmage/common/FileSearch.h>
+#include <scrimmage/common/Utilities.h>
+#include <scrimmage/entity/EntityPlugin.h>
 #include <scrimmage/parse/ConfigParse.h>
 #include <scrimmage/parse/ParseUtils.h>
 #include <scrimmage/plugin_manager/PluginManager.h>
-#include <scrimmage/entity/EntityPlugin.h>
 
+#include <boost/filesystem.hpp>
 #include <iostream>
 #include <set>
 
-#include <boost/filesystem.hpp>
-
 namespace scrimmage {
 
 int PluginManager::check_library(std::string lib_path) {
-    void *lib_handle;
+    void* lib_handle;
 #ifdef __APPLE__
     lib_handle = dlopen(lib_path.c_str(), RTLD_LAZY | RTLD_GLOBAL);
 #else
@@ -60,12 +58,12 @@ int PluginManager::check_library(std::string lib_path) {
         return -1;
     }
 
-    char * error;
+    char* error;
 
     // Extract the plugin type
     plugin_type_t type_func;
     type_func = (plugin_type_t)dlsym(lib_handle, "plugin_type");
-    if ((error = dlerror()) != NULL)  {
+    if ((error = dlerror()) != NULL) {
         // fputs(error, stderr);
         std::cout << lib_path << " doesn't contain 'plugin_type'" << std::endl;
         dlclose(lib_handle);
@@ -76,7 +74,7 @@ int PluginManager::check_library(std::string lib_path) {
     // Extract the plugin name
     plugin_name_t name_func;
     name_func = (plugin_name_t)dlsym(lib_handle, "plugin_name");
-    if ((error = dlerror()) != NULL)  {
+    if ((error = dlerror()) != NULL) {
         // fputs(error, stderr);
         std::cout << lib_path << " doesn't contain 'plugin_name'" << std::endl;
         dlclose(lib_handle);
@@ -86,7 +84,7 @@ int PluginManager::check_library(std::string lib_path) {
 
     // Ensure the maker function exists, but we don't need to call it
     dlsym(lib_handle, "maker");
-    if ((error = dlerror()) != NULL)  {
+    if ((error = dlerror()) != NULL) {
         fputs(error, stderr);
         std::cout << lib_path << " doesn't contain 'maker'" << std::endl;
         dlclose(lib_handle);
@@ -106,20 +104,19 @@ int PluginManager::check_library(std::string lib_path) {
     return 0;
 }
 
-PluginManager::PluginManager() : reload_(false) {}
+PluginManager::PluginManager() : reload_(false) {
+}
 
-void PluginManager::print_plugins(const std::string &plugin_type,
-                                  const std::string &title,
-                                  FileSearch &file_search,
-                                  const std::string &env_var_name) {
+void PluginManager::print_plugins(const std::string& plugin_type, const std::string& title,
+                                  FileSearch& file_search, const std::string& env_var_name) {
     // make sure all files are loaded
     if (!files_checked_) {
         file_search.find_files(env_var_name, LIB_EXT, so_files_);
         files_checked_ = true;
     }
 
-    for (auto &kv : so_files_) {
-        for (std::string &full_path : kv.second) {
+    for (auto& kv : so_files_) {
+        for (std::string& full_path : kv.second) {
             check_library(full_path);
         }
     }
@@ -128,7 +125,7 @@ void PluginManager::print_plugins(const std::string &plugin_type,
     std::cout << title << ": " << std::endl;
     std::cout << "------------------------------" << std::endl;
     if (plugins_info_.count(plugin_type) > 0) {
-        for (auto &kv : plugins_info_[plugin_type]) {
+        for (auto& kv : plugins_info_[plugin_type]) {
             std::cout << kv.first << std::endl;
         }
     } else {
@@ -139,8 +136,8 @@ void PluginManager::print_plugins(const std::string &plugin_type,
 
 void PluginManager::print_returned_plugins() {
     std::cout << "using the following plugins:" << std::endl;
-    for (auto &kv : plugins_info_) {
-        for (auto &kv2 : kv.second) {
+    for (auto& kv : plugins_info_) {
+        for (auto& kv2 : kv.second) {
             if (kv2.second.returned) {
                 std::cout << kv.first << "::" << kv2.first << std::endl;
             }
@@ -148,7 +145,7 @@ void PluginManager::print_returned_plugins() {
     }
 }
 
-PluginPtr PluginManager::make_plugin_helper(std::string &plugin_type, std::string &plugin_name) {
+PluginPtr PluginManager::make_plugin_helper(std::string& plugin_type, std::string& plugin_name) {
     auto it = plugins_info_.find(plugin_type);
     if (it != plugins_info_.end()) {
         auto it2 = it->second.find(plugin_name);
@@ -162,8 +159,8 @@ PluginPtr PluginManager::make_plugin_helper(std::string &plugin_type, std::strin
             PluginPtr (*maker_func)(void);
             // cppcheck-suppress cstyleCast
             maker_func = (PluginPtr (*)(void))dlsym(it2->second.handle, "maker");
-            char * error;
-            if ((error = dlerror()) != NULL)  {
+            char* error;
+            if ((error = dlerror()) != NULL) {
                 fputs(error, stderr);
                 return nullptr;
             } else {
@@ -177,13 +174,12 @@ PluginPtr PluginManager::make_plugin_helper(std::string &plugin_type, std::strin
 std::map<std::string, std::unordered_set<std::string>> PluginManager::get_commits() {
     std::map<std::string, std::unordered_set<std::string>> commits;
     std::string sha;
-    for (auto &kv : plugins_info_) {
-        for (auto &kv2 : kv.second) {
-            scrimmage::PluginInfo &plugin_info = kv2.second;
+    for (auto& kv : plugins_info_) {
+        for (auto& kv2 : kv.second) {
+            scrimmage::PluginInfo& plugin_info = kv2.second;
             if (!plugin_info.returned) continue;
 
-            std::string path =
-                boost::filesystem::path(plugin_info.path).parent_path().string();
+            std::string path = boost::filesystem::path(plugin_info.path).parent_path().string();
 
             if ((sha = scrimmage::get_sha(path)) != "") {
                 commits[sha].insert(plugin_info.name);
@@ -194,13 +190,12 @@ std::map<std::string, std::unordered_set<std::string>> PluginManager::get_commit
 }
 
 void PluginManager::find_matching_plugins(
-    const std::string &plugin_name_so,
-    std::unordered_map<std::string, std::list<std::string>> &so_files,
-    std::list<std::string> &plugins) {
-
+    const std::string& plugin_name_so,
+    std::unordered_map<std::string, std::list<std::string>>& so_files,
+    std::list<std::string>& plugins) {
     auto it = so_files.find(std::string("lib") + plugin_name_so + LIB_EXT);
     if (it != so_files.end()) {
-        for (std::string &full_fname : it->second) {
+        for (std::string& full_fname : it->second) {
             if (check_library(full_fname) == 0) {
                 plugins.push_back(full_fname);
             }
@@ -211,8 +206,12 @@ void PluginManager::find_matching_plugins(
     }
 }
 
-void PluginManager::set_reload(bool reload) {reload_ = reload;}
+void PluginManager::set_reload(bool reload) {
+    reload_ = reload;
+}
 
-bool PluginManager::get_reload() {return reload_;}
+bool PluginManager::get_reload() {
+    return reload_;
+}
 
-} // namespace scrimmage
+}  // namespace scrimmage
diff --git a/src/plugins/autonomy/APITester/APITester.cpp b/src/plugins/autonomy/APITester/APITester.cpp
index 94d67ab5b3..cbe693ef90 100644
--- a/src/plugins/autonomy/APITester/APITester.cpp
+++ b/src/plugins/autonomy/APITester/APITester.cpp
@@ -30,13 +30,12 @@
  *
  */
 
-#include <scrimmage/plugins/autonomy/APITester/APITester.h>
-
-#include <scrimmage/plugin_manager/RegisterPlugin.h>
 #include <scrimmage/entity/Entity.h>
 #include <scrimmage/math/State.h>
-#include <scrimmage/parse/ParseUtils.h>
 #include <scrimmage/parse/MissionParse.h>
+#include <scrimmage/parse/ParseUtils.h>
+#include <scrimmage/plugin_manager/RegisterPlugin.h>
+#include <scrimmage/plugins/autonomy/APITester/APITester.h>
 
 #include <iostream>
 #include <limits>
@@ -46,27 +45,25 @@ using std::endl;
 
 namespace sc = scrimmage;
 
-REGISTER_PLUGIN(scrimmage::Autonomy,
-                scrimmage::autonomy::APITester,
-                APITester_plugin)
+REGISTER_PLUGIN(scrimmage::Autonomy, scrimmage::autonomy::APITester, APITester_plugin)
 
 namespace scrimmage {
 namespace autonomy {
 
 void APITester::write_my_test_values() {
-    csv_.append(CSV::Pairs{
-            {"my_test_bool", my_test_bool_},
-            {"my_test_int", my_test_int_},
-            {"my_test_float", my_test_float_},
-            {"my_test_double", my_test_double_}
-        });
+    csv_.append(CSV::Pairs{{"my_test_bool", my_test_bool_},
+                           {"my_test_int", my_test_int_},
+                           {"my_test_float", my_test_float_},
+                           {"my_test_double", my_test_double_}});
 }
 
-void APITester::init(std::map<std::string, std::string> &params) {
+void APITester::init(std::map<std::string, std::string>& params) {
     // Setup variableIO
-    desired_alt_idx_ = vars_.declare(VariableIO::Type::desired_altitude, VariableIO::Direction::Out);
+    desired_alt_idx_ =
+        vars_.declare(VariableIO::Type::desired_altitude, VariableIO::Direction::Out);
     desired_speed_idx_ = vars_.declare(VariableIO::Type::desired_speed, VariableIO::Direction::Out);
-    desired_heading_idx_ = vars_.declare(VariableIO::Type::desired_heading, VariableIO::Direction::Out);
+    desired_heading_idx_ =
+        vars_.declare(VariableIO::Type::desired_heading, VariableIO::Direction::Out);
 
     // Get default variable values
     my_test_bool_ = sc::get<bool>("my_test_bool", params, my_test_bool_);
@@ -81,32 +78,24 @@ void APITester::init(std::map<std::string, std::string> &params) {
         std::cout << "APITest: Couldn't create output file" << endl;
     }
     csv_.set_column_headers("my_test_bool, my_test_int, my_test_float, my_test_double");
-    write_my_test_values(); // Write initial values to csv
+    write_my_test_values();  // Write initial values to csv
 
     // Register the test variables with the parameter server
-    auto bool_cb = [&](const bool &my_test_bool) {
-        this->write_my_test_values();
-    };
+    auto bool_cb = [&](const bool& my_test_bool) { this->write_my_test_values(); };
     register_param<bool>("my_test_bool", my_test_bool_, bool_cb);
 
-    auto int_cb = [&](const int &my_test_int) {
-        this->write_my_test_values();
-    };
+    auto int_cb = [&](const int& my_test_int) { this->write_my_test_values(); };
     register_param<int>("my_test_int", my_test_int_, int_cb);
 
-    auto float_cb = [&](const float &my_test_float) {
-        this->write_my_test_values();
-    };
+    auto float_cb = [&](const float& my_test_float) { this->write_my_test_values(); };
     register_param<float>("my_test_float", my_test_float_, float_cb);
 
-    auto double_cb = [&](const double &my_test_double) {
-        this->write_my_test_values();
-    };
+    auto double_cb = [&](const double& my_test_double) { this->write_my_test_values(); };
     register_param<double>("my_test_double", my_test_double_, double_cb);
 }
 
 bool APITester::step_autonomy(double t, double dt) {
     return true;
 }
-} // namespace autonomy
-} // namespace scrimmage
+}  // namespace autonomy
+}  // namespace scrimmage
diff --git a/src/plugins/autonomy/ArduPilot/ArduPilot.cpp b/src/plugins/autonomy/ArduPilot/ArduPilot.cpp
index 3fff4b7c1e..15cb5832e3 100644
--- a/src/plugins/autonomy/ArduPilot/ArduPilot.cpp
+++ b/src/plugins/autonomy/ArduPilot/ArduPilot.cpp
@@ -29,52 +29,45 @@
  *
  */
 
-#include <scrimmage/plugins/autonomy/ArduPilot/ArduPilot.h>
-
-#include <scrimmage/plugin_manager/RegisterPlugin.h>
 #include <scrimmage/entity/Entity.h>
 #include <scrimmage/math/Angles.h>
 #include <scrimmage/math/State.h>
-#include <scrimmage/parse/ParseUtils.h>
 #include <scrimmage/parse/MissionParse.h>
+#include <scrimmage/parse/ParseUtils.h>
+#include <scrimmage/plugin_manager/RegisterPlugin.h>
+#include <scrimmage/plugins/autonomy/ArduPilot/ArduPilot.h>
 #include <scrimmage/sensor/Sensor.h>
 
+#include <GeographicLib/LocalCartesian.hpp>
+#include <boost/asio/basic_datagram_socket.hpp>
+#include <boost/asio/buffer.hpp>
+#include <boost/asio/placeholders.hpp>
+#include <boost/bind.hpp>
+#include <chrono>
+#include <fstream>
 #include <iomanip>
 #include <iostream>
 #include <limits>
-#include <fstream>
-#include <chrono>
 
-#include <GeographicLib/LocalCartesian.hpp>
-
-#include <boost/bind.hpp>
-#include <boost/asio/placeholders.hpp>
-#include <boost/asio/buffer.hpp>
-#include <boost/asio/basic_datagram_socket.hpp>
-
-using std::cout;
 using std::cerr;
+using std::cout;
 using std::endl;
 
 namespace ba = boost::asio;
 namespace sc = scrimmage;
 
-REGISTER_PLUGIN(scrimmage::Autonomy,
-                scrimmage::autonomy::ArduPilot,
-                ArduPilot_plugin)
+REGISTER_PLUGIN(scrimmage::Autonomy, scrimmage::autonomy::ArduPilot, ArduPilot_plugin)
 
 namespace scrimmage {
 namespace autonomy {
 
-ArduPilot::ArduPilot() :
-    angles_to_gps_(0, Angles::Type::EUCLIDEAN, Angles::Type::GPS) {
-
+ArduPilot::ArduPilot() : angles_to_gps_(0, Angles::Type::EUCLIDEAN, Angles::Type::GPS) {
     for (int i = 0; i < MAX_NUM_SERVOS; i++) {
         servo_pkt_.servos[i] = 0;
     }
 }
 
-void ArduPilot::init(std::map<std::string, std::string> &params) {
+void ArduPilot::init(std::map<std::string, std::string>& params) {
     std::string servo_map = sc::get<std::string>("servo_map", params, "");
     std::vector<std::vector<std::string>> vecs;
     if (!sc::get_vec_of_vecs(servo_map, vecs)) {
@@ -91,13 +84,12 @@ void ArduPilot::init(std::map<std::string, std::string> &params) {
 
             int servo = std::stod(vec[1]);
             if (servo >= MAX_NUM_SERVOS) {
-                cout << "Warning: servo_map contains out-of-range servo index"
-                     << endl;
+                cout << "Warning: servo_map contains out-of-range servo index" << endl;
             } else {
-                scrimmage::controller::AxisScale at(servo, std::stod(vec[2]),
-                             std::stod(vec[3]), std::stod(vec[4]),
-                             std::stod(vec[5]), std::stod(vec[6]),
-                             vars_.declare(vec[0], VariableIO::Direction::Out));
+                scrimmage::controller::AxisScale at(
+                    servo, std::stod(vec[2]), std::stod(vec[3]), std::stod(vec[4]),
+                    std::stod(vec[5]), std::stod(vec[6]),
+                    vars_.declare(vec[0], VariableIO::Direction::Out));
                 servo_tfs_.push_back(at);
 
                 // cout << "Mapping servo " << at.axis_index() << " to channel " <<
@@ -110,8 +102,7 @@ void ArduPilot::init(std::map<std::string, std::string> &params) {
     asynchonous_mode_ = sc::get<bool>("asynchronous_mode", params, true);
 
     // Get parameters for transmit socket (to ardupilot)
-    std::string to_ardupilot_ip = sc::get<std::string>("to_ardupilot_ip",
-                                                       params, "127.0.0.1");
+    std::string to_ardupilot_ip = sc::get<std::string>("to_ardupilot_ip", params, "127.0.0.1");
 
     int to_ardupilot_port = sc::get<int>("to_ardupilot_port", params, 5003);
     int from_ardupilot_port = sc::get<int>("from_ardupilot_port", params, 5002);
@@ -123,32 +114,27 @@ void ArduPilot::init(std::map<std::string, std::string> &params) {
         int multiplier = sc::get<int>("port_instance_multiplier", params, 10);
 
         int base_port = convert<int>(it_base_port->second);
-        from_ardupilot_port = base_port + multiplier*parent_->id().id();
+        from_ardupilot_port = base_port + multiplier * parent_->id().id();
         to_ardupilot_port = from_ardupilot_port + 1;
     }
 
-    cout << "ArduPilot: sending to udp: " << to_ardupilot_ip << ":"
-         << to_ardupilot_port << endl;
-    cout << "ArduPilot: listening to udp: " << to_ardupilot_ip << ":"
-         << from_ardupilot_port << endl;
+    cout << "ArduPilot: sending to udp: " << to_ardupilot_ip << ":" << to_ardupilot_port << endl;
+    cout << "ArduPilot: listening to udp: " << to_ardupilot_ip << ":" << from_ardupilot_port
+         << endl;
 
     // Setup transmit socket
     tx_socket_ = std::make_shared<ba::ip::udp::socket>(tx_io_service_,
-                                                       ba::ip::udp::endpoint(
-                                                           ba::ip::udp::v4(),
-                                                           0));
+                                                       ba::ip::udp::endpoint(ba::ip::udp::v4(), 0));
     tx_resolver_ = std::make_shared<ba::ip::udp::resolver>(tx_io_service_);
-    tx_endpoint_ = *(tx_resolver_->resolve({ba::ip::udp::v4(),
-                        to_ardupilot_ip, std::to_string(to_ardupilot_port)}));
+    tx_endpoint_ = *(tx_resolver_->resolve(
+        {ba::ip::udp::v4(), to_ardupilot_ip, std::to_string(to_ardupilot_port)}));
 
-    recv_socket_ = std::make_shared<ba::ip::udp::socket>(recv_io_service_,
-                                                         ba::ip::udp::endpoint(
-                                                             ba::ip::udp::v4(),
-                                                             from_ardupilot_port));
+    recv_socket_ = std::make_shared<ba::ip::udp::socket>(
+        recv_io_service_, ba::ip::udp::endpoint(ba::ip::udp::v4(), from_ardupilot_port));
     start_receive();
 
     state_6dof_ = std::make_shared<motion::RigidBody6DOFState>();
-    auto cb = [&](auto &msg){*state_6dof_ = msg->data;};
+    auto cb = [&](auto& msg) { *state_6dof_ = msg->data; };
     subscribe<motion::RigidBody6DOFState>("LocalNetwork", "RigidBody6DOFState", cb);
 }
 
@@ -161,8 +147,7 @@ void ArduPilot::start_receive() {
     // Enable the receive async callback
     recv_socket_->async_receive_from(
         ba::buffer(recv_buffer_), recv_remote_endpoint_,
-        boost::bind(&ArduPilot::handle_receive, this,
-                    ba::placeholders::error,
+        boost::bind(&ArduPilot::handle_receive, this, ba::placeholders::error,
                     ba::placeholders::bytes_transferred));
 }
 
@@ -172,13 +157,11 @@ void ArduPilot::close(double t) {
 }
 
 bool ArduPilot::step_autonomy(double t, double dt) {
-
     // Convert state6dof into ArduPilot fdm_packet and transmit
     fdm_packet fdm_pkt = state6dof_to_fdm_packet(t, *state_6dof_);
     try {
         // TODO: Michael, endian handling?
-        tx_socket_->send_to(ba::buffer(&fdm_pkt, sizeof(fdm_packet)),
-                            tx_endpoint_);
+        tx_socket_->send_to(ba::buffer(&fdm_pkt, sizeof(fdm_packet)), tx_endpoint_);
         ntx++;
         last_ntx++;
     } catch (std::exception& e) {
@@ -191,9 +174,8 @@ bool ArduPilot::step_autonomy(double t, double dt) {
     } else {
         boost::system::error_code error;
         ba::ip::udp::socket::message_flags flags = 0;
-        std::size_t nbytes = recv_socket_->receive_from(
-            ba::buffer(recv_buffer_), recv_remote_endpoint_,
-            flags, error);
+        std::size_t nbytes = recv_socket_->receive_from(ba::buffer(recv_buffer_),
+                                                        recv_remote_endpoint_, flags, error);
 
         parse_receive(error, nbytes);
     }
@@ -201,22 +183,20 @@ bool ArduPilot::step_autonomy(double t, double dt) {
     // Copy the received servo commands into the desired state
     servo_pkt_mutex_.lock();
     for (auto servo_tf : servo_tfs_) {
-        vars_.output(servo_tf.vector_index(), servo_tf.scale(servo_pkt_.servos[servo_tf.axis_index()]));
+        vars_.output(servo_tf.vector_index(),
+                     servo_tf.scale(servo_pkt_.servos[servo_tf.axis_index()]));
         // int prec = 9;
-        // cout << std::setprecision(prec) << "servo_out"<< servo_tf.axis_index() << ": " << servo_tf.scale(servo_pkt_.servos[servo_tf.axis_index()]) << endl;
+        // cout << std::setprecision(prec) << "servo_out"<< servo_tf.axis_index() << ": " <<
+        // servo_tf.scale(servo_pkt_.servos[servo_tf.axis_index()]) << endl;
     }
     servo_pkt_mutex_.unlock();
 
-
     if (t - last_print_t > 5) {
         const auto t0 = std::chrono::system_clock::now();
-        auto d = std::chrono::duration_cast<std::chrono::microseconds>(t0-last_print_wall_t);
-        cout <<
-        "walltime: " << t0.time_since_epoch().count() <<
-        ", simtime: " << t <<
-        ", packets tx: " << ntx << ", rx: " << nrx <<
-        ", rate: " << double(last_ntx) / double(d.count()) * 1e6 << " hz" <<
-        endl;
+        auto d = std::chrono::duration_cast<std::chrono::microseconds>(t0 - last_print_wall_t);
+        cout << "walltime: " << t0.time_since_epoch().count() << ", simtime: " << t
+             << ", packets tx: " << ntx << ", rx: " << nrx
+             << ", rate: " << double(last_ntx) / double(d.count()) * 1e6 << " hz" << endl;
         last_print_t = t;
         last_print_wall_t = t0;
         last_ntx = 0;
@@ -225,8 +205,7 @@ bool ArduPilot::step_autonomy(double t, double dt) {
     return true;
 }
 
-void ArduPilot::parse_receive(const boost::system::error_code& error,
-                               std::size_t num_bytes) {
+void ArduPilot::parse_receive(const boost::system::error_code& error, std::size_t num_bytes) {
 #if 0
     cout << "--------------------------------------------------------" << endl;
     cout << "  Servo packets received from ArduPilot (ID: "
@@ -242,7 +221,7 @@ void ArduPilot::parse_receive(const boost::system::error_code& error,
     } else {
         servo_pkt_mutex_.lock();
         for (unsigned int i = 0; i < num_bytes / sizeof(uint16_t); i++) {
-            servo_pkt_.servos[i] = (recv_buffer_[i*2+1] << 8) + recv_buffer_[i*2];
+            servo_pkt_.servos[i] = (recv_buffer_[i * 2 + 1] << 8) + recv_buffer_[i * 2];
 #if 0
             int prec = 9;
             cout << std::setprecision(prec) << "servo"<< i << ": " << servo_pkt_.servos[i] << endl;
@@ -253,24 +232,20 @@ void ArduPilot::parse_receive(const boost::system::error_code& error,
     nrx++;
 }
 
-void ArduPilot::handle_receive(const boost::system::error_code& error,
-                               std::size_t num_bytes) {
+void ArduPilot::handle_receive(const boost::system::error_code& error, std::size_t num_bytes) {
     parse_receive(error, num_bytes);
-    start_receive(); // enable async receive for next message
+    start_receive();  // enable async receive for next message
 }
 
-ArduPilot::fdm_packet ArduPilot::state6dof_to_fdm_packet(
-    double t, sc::motion::RigidBody6DOFState &state) {
-
+ArduPilot::fdm_packet ArduPilot::state6dof_to_fdm_packet(double t,
+                                                         sc::motion::RigidBody6DOFState& state) {
     fdm_packet fdm_pkt = {0};
 
     fdm_pkt.timestamp_us = t * 1e6;
 
     // x/y/z to lat/lon/alt conversion
-    parent_->projection()->Reverse(state.pos()(0), state.pos()(1),
-                                   state.pos()(2), fdm_pkt.latitude,
-                                   fdm_pkt.longitude,
-                                   fdm_pkt.altitude);
+    parent_->projection()->Reverse(state.pos()(0), state.pos()(1), state.pos()(2), fdm_pkt.latitude,
+                                   fdm_pkt.longitude, fdm_pkt.altitude);
 
     // convert everything to NED-FRU world and body frames
 
@@ -284,8 +259,8 @@ ArduPilot::fdm_packet ArduPilot::state6dof_to_fdm_packet(
 
     // Body frame linear acceleration FRU
     if (mavproxy_mode_) {
-        Eigen::Vector3d specific_force_body = state.linear_accel_body()
-                    - state.quat().rotate_reverse(Eigen::Vector3d(0, 0, -9.81));
+        Eigen::Vector3d specific_force_body =
+            state.linear_accel_body() - state.quat().rotate_reverse(Eigen::Vector3d(0, 0, -9.81));
         fdm_pkt.xAccel = specific_force_body(0);
         fdm_pkt.yAccel = -specific_force_body(1);
         fdm_pkt.zAccel = -specific_force_body(2);
@@ -302,25 +277,23 @@ ArduPilot::fdm_packet ArduPilot::state6dof_to_fdm_packet(
 
     if (mavproxy_mode_) {
         // world frame rotational velocities
-        double p =  state.ang_vel_body()(0);
+        double p = state.ang_vel_body()(0);
         double q = -state.ang_vel_body()(1);
         double r = -state.ang_vel_body()(2);
         double sphi = sin(fdm_pkt.roll);
         double cphi = cos(fdm_pkt.roll);
         double tth = tan(fdm_pkt.pitch);
         double cth = std::max(.00001, cos(fdm_pkt.pitch));
-        fdm_pkt.rollRate  = p +  tth*sphi*q + tth*cphi*r;
-        fdm_pkt.pitchRate =          cphi*q -     sphi*r;
-        fdm_pkt.yawRate   =     sphi/cth *q + cphi/cth*r;
+        fdm_pkt.rollRate = p + tth * sphi * q + tth * cphi * r;
+        fdm_pkt.pitchRate = cphi * q - sphi * r;
+        fdm_pkt.yawRate = sphi / cth * q + cphi / cth * r;
     } else {
         // Body frame rotational velocities FRU
-        fdm_pkt.rollRate  = state.ang_vel_body()(0);
+        fdm_pkt.rollRate = state.ang_vel_body()(0);
         fdm_pkt.pitchRate = -state.ang_vel_body()(1);
-        fdm_pkt.yawRate   = -state.ang_vel_body()(2);
+        fdm_pkt.yawRate = -state.ang_vel_body()(2);
     }
 
-
-
     // Airspeed is magnitude of velocity vector for now
     fdm_pkt.airspeed = (state.vel() + state.wind()).norm();
     if (std::isnan(fdm_pkt.airspeed)) {
@@ -362,5 +335,5 @@ ArduPilot::fdm_packet ArduPilot::state6dof_to_fdm_packet(
     return fdm_pkt;
 }
 
-} // namespace autonomy
-} // namespace scrimmage
+}  // namespace autonomy
+}  // namespace scrimmage
diff --git a/src/plugins/autonomy/AuctionAssign/AuctionAssign.cpp b/src/plugins/autonomy/AuctionAssign/AuctionAssign.cpp
index 8e710e9c60..43927152b3 100644
--- a/src/plugins/autonomy/AuctionAssign/AuctionAssign.cpp
+++ b/src/plugins/autonomy/AuctionAssign/AuctionAssign.cpp
@@ -30,19 +30,18 @@
  *
  */
 
-#include <scrimmage/plugin_manager/RegisterPlugin.h>
-#include <scrimmage/entity/Entity.h>
 #include <scrimmage/common/Random.h>
 #include <scrimmage/common/Time.h>
-#include <scrimmage/parse/ParseUtils.h>
+#include <scrimmage/entity/Entity.h>
 #include <scrimmage/math/State.h>
+#include <scrimmage/msgs/AuctionMsgs.pb.h>
+#include <scrimmage/parse/ParseUtils.h>
+#include <scrimmage/plugin_manager/RegisterPlugin.h>
+#include <scrimmage/plugins/autonomy/AuctionAssign/AuctionAssign.h>
+#include <scrimmage/pubsub/Message.h>
 #include <scrimmage/pubsub/Network.h>
 #include <scrimmage/pubsub/Publisher.h>
 #include <scrimmage/pubsub/Subscriber.h>
-#include <scrimmage/pubsub/Message.h>
-
-#include <scrimmage/plugins/autonomy/AuctionAssign/AuctionAssign.h>
-#include <scrimmage/msgs/AuctionMsgs.pb.h>
 
 #include <iostream>
 #include <limits>
@@ -60,13 +59,13 @@ REGISTER_PLUGIN(scrimmage::Autonomy, scrimmage::autonomy::AuctionAssign, Auction
 namespace scrimmage {
 namespace autonomy {
 
-void AuctionAssign::init(std::map<std::string, std::string> &params) {
+void AuctionAssign::init(std::map<std::string, std::string>& params) {
     id_ = parent_->id().id();
     auctioneer_ = str2bool(params.at("auctioneer"));
 
     desired_state_->vel() << 0, 0, 0;
     desired_state_->quat().set(0, 0, state_->quat().yaw());
-    desired_state_->pos() = Eigen::Vector3d::UnitZ()*state_->pos()(2);
+    desired_state_->pos() = Eigen::Vector3d::UnitZ() * state_->pos()(2);
 
     // Setup Publishers
     start_auction_pub_ = advertise("CommsNetwork", "StartAuction");
@@ -74,13 +73,11 @@ void AuctionAssign::init(std::map<std::string, std::string> &params) {
     result_auction_pub_ = advertise("CommsNetwork", "ResultAuction");
 
     // Setup the lambda function to process the StartAuction message
-    auto start_auction_callback = [&]
-        (scrimmage::MessagePtr<auction::StartAuction> msg) {
+    auto start_auction_callback = [&](scrimmage::MessagePtr<auction::StartAuction> msg) {
         cout << "-----------------------------" << endl;
         cout << "Time: " << time_->t() << endl;
         cout << "StartAuction: entity ID (" << id_ << ")"
-             << " received message from entity ID: " << msg->data.sender_id()
-             << endl;
+             << " received message from entity ID: " << msg->data.sender_id() << endl;
 
         auto msg_bid = std::make_shared<sc::Message<auction::BidAuction>>();
         msg_bid->data.set_sender_id(id_);
@@ -91,17 +88,14 @@ void AuctionAssign::init(std::map<std::string, std::string> &params) {
     };
 
     // Subscribe to the StartAuction topic
-    subscribe<auction::StartAuction>("CommsNetwork", "StartAuction",
-                                     start_auction_callback);
+    subscribe<auction::StartAuction>("CommsNetwork", "StartAuction", start_auction_callback);
 
     // Setup the lambda function to process the BidAuction messages
-    auto bid_auction_callback = [&]
-        (scrimmage::MessagePtr<auction::BidAuction> msg) {
+    auto bid_auction_callback = [&](scrimmage::MessagePtr<auction::BidAuction> msg) {
         cout << "-----------------------------" << endl;
         cout << "Time: " << time_->t() << endl;
         cout << "BidAuction: entity ID (" << id_ << ") received message from "
-             << "entity ID (" << msg->data.sender_id() << "),  bid: "
-             << msg->data.bid() << endl;
+             << "entity ID (" << msg->data.sender_id() << "),  bid: " << msg->data.bid() << endl;
 
         if (msg->data.bid() > max_bid_) {
             max_bid_ = msg->data.bid();
@@ -110,8 +104,7 @@ void AuctionAssign::init(std::map<std::string, std::string> &params) {
     };
 
     // Subscribe to the BidAuction topic
-    subscribe<auction::BidAuction>("CommsNetwork", "BidAuction",
-                                   bid_auction_callback);
+    subscribe<auction::BidAuction>("CommsNetwork", "BidAuction", bid_auction_callback);
 
     output_vel_x_idx_ = vars_.declare(VariableIO::Type::velocity_x, VariableIO::Direction::Out);
     output_vel_y_idx_ = vars_.declare(VariableIO::Type::velocity_y, VariableIO::Direction::Out);
@@ -131,8 +124,7 @@ bool AuctionAssign::step_autonomy(double t, double dt) {
         auction_start_time_ = t;
     }
 
-    if (auction_in_prog_ && t > auction_start_time_ + auction_max_time_
-        && id_ == 1) {
+    if (auction_in_prog_ && t > auction_start_time_ + auction_max_time_ && id_ == 1) {
         cout << "======================================" << endl;
         cout << "Auction Complete" << endl;
         cout << "Max Bidder: " << max_bid_champ_ << endl;
@@ -147,5 +139,5 @@ bool AuctionAssign::step_autonomy(double t, double dt) {
 
     return true;
 }
-} // namespace autonomy
-} // namespace scrimmage
+}  // namespace autonomy
+}  // namespace scrimmage
diff --git a/src/plugins/autonomy/AutonomyExecutor/AutonomyExecutor.cpp b/src/plugins/autonomy/AutonomyExecutor/AutonomyExecutor.cpp
index 1247db2ec5..53e687c72c 100644
--- a/src/plugins/autonomy/AutonomyExecutor/AutonomyExecutor.cpp
+++ b/src/plugins/autonomy/AutonomyExecutor/AutonomyExecutor.cpp
@@ -30,41 +30,37 @@
  *
  */
 
-#include <scrimmage/plugins/autonomy/AutonomyExecutor/AutonomyExecutor.h>
-
-#include <scrimmage/plugin_manager/RegisterPlugin.h>
+#include <scrimmage/common/Time.h>
 #include <scrimmage/entity/Entity.h>
 #include <scrimmage/math/State.h>
 #include <scrimmage/parse/ParseUtils.h>
 #include <scrimmage/plugin_manager/PluginManager.h>
-#include <scrimmage/proto/Shape.pb.h>
+#include <scrimmage/plugin_manager/RegisterPlugin.h>
+#include <scrimmage/plugins/autonomy/AutonomyExecutor/AutonomyExecutor.h>
 #include <scrimmage/proto/ProtoConversions.h>
-#include <scrimmage/common/Time.h>
+#include <scrimmage/proto/Shape.pb.h>
 
+#include <boost/algorithm/string/classification.hpp>
+#include <boost/algorithm/string/split.hpp>
 #include <iostream>
 #include <limits>
 
-#include <boost/algorithm/string/split.hpp>
-#include <boost/algorithm/string/classification.hpp>
-
 using std::cout;
 using std::endl;
 
 namespace sc = scrimmage;
 
-REGISTER_PLUGIN(scrimmage::Autonomy,
-                scrimmage::autonomy::AutonomyExecutor,
-                AutonomyExecutor_plugin)
+REGISTER_PLUGIN(scrimmage::Autonomy, scrimmage::autonomy::AutonomyExecutor, AutonomyExecutor_plugin)
 
 namespace scrimmage {
 namespace autonomy {
 
-void AutonomyExecutor::init(std::map<std::string, std::string> &params) {
+void AutonomyExecutor::init(std::map<std::string, std::string>& params) {
     show_shapes_ = sc::get("show_shapes", params, false);
 
     // Discover the output variables that point to the input variables for the
     // motion model. Setup input variables that mirror the output variables.
-    for (auto &kv : vars_.output_variable_index()) {
+    for (auto& kv : vars_.output_variable_index()) {
         int out_idx = vars_.declare(kv.first, VariableIO::Direction::Out);
         int in_idx = vars_.declare(kv.first, VariableIO::Direction::In);
         io_map_[out_idx] = in_idx;
@@ -74,7 +70,7 @@ void AutonomyExecutor::init(std::map<std::string, std::string> &params) {
     std::string state_topic_name = sc::get<std::string>("state_topic_name", params, "State");
     std::string network_name = sc::get<std::string>("network_name", params, "LocalNetwork");
 
-    auto state_callback = [&] (scrimmage::MessagePtr<std::string> msg) {
+    auto state_callback = [&](scrimmage::MessagePtr<std::string> msg) {
         // If the state didn't change, return immediately.
         if (current_state_ == msg->data) {
             return;
@@ -94,8 +90,7 @@ void AutonomyExecutor::init(std::map<std::string, std::string> &params) {
         // running.
         for (sc::AutonomyPtr autonomy : running_autonomies_) {
             if (!call_init(autonomy->name(), autonomy)) {
-                cout << "AutonomyExecutor: Failed to reload init for "
-                     << autonomy->name() << endl;
+                cout << "AutonomyExecutor: Failed to reload init for " << autonomy->name() << endl;
             }
         }
     };
@@ -125,11 +120,8 @@ void AutonomyExecutor::init(std::map<std::string, std::string> &params) {
                 if (tokens.size() == 2) {
                     // Remove the quotes from the value
                     tokens[1].erase(
-                        std::remove_if(tokens[1].begin(),
-                                       tokens[1].end(),
-                                       [](unsigned char x){
-                                           return (x == '\"') || (x == '\'');
-                                       }),
+                        std::remove_if(tokens[1].begin(), tokens[1].end(),
+                                       [](unsigned char x) { return (x == '\"') || (x == '\''); }),
                         tokens[1].end());
                     autonomy_params[tokens[0]] = tokens[1];
                 }
@@ -138,16 +130,11 @@ void AutonomyExecutor::init(std::map<std::string, std::string> &params) {
         }
 
         sc::ConfigParse config_parse;
-        PluginStatus<Autonomy> status =
-            parent_->plugin_manager()->make_plugin<Autonomy>("scrimmage::Autonomy",
-                                                             autonomy_name,
-                                                             *(parent_->file_search()),
-                                                             config_parse,
-                                                             autonomy_params,
-                                                             std::set<std::string>{});
+        PluginStatus<Autonomy> status = parent_->plugin_manager()->make_plugin<Autonomy>(
+            "scrimmage::Autonomy", autonomy_name, *(parent_->file_search()), config_parse,
+            autonomy_params, std::set<std::string>{});
         if (status.status == PluginStatus<Autonomy>::cast_failed) {
-            cout << "AutonomyExecutor Failed to open autonomy plugin: "
-                 << autonomy_name << endl;
+            cout << "AutonomyExecutor Failed to open autonomy plugin: " << autonomy_name << endl;
         } else if (status.status == PluginStatus<Autonomy>::loaded) {
             // connect the initialized autonomy plugin's variable output with
             // the variable input to the controller. This is similar to
@@ -186,7 +173,7 @@ void AutonomyExecutor::init(std::map<std::string, std::string> &params) {
 
     // Add the default autonomies to each declared state
     for (scrimmage::AutonomyPtr autonomy : default_autonomies_) {
-        for (auto &kv : autonomies_) {
+        for (auto& kv : autonomies_) {
             kv.second.push_back(autonomy);
         }
     }
@@ -199,7 +186,7 @@ bool AutonomyExecutor::step_autonomy(double t, double dt) {
         // cout << "Running... " << autonomy->name() << endl;
         autonomy->shapes().clear();
 
-        for (SubscriberBasePtr &sub : autonomy->subs()) {
+        for (SubscriberBasePtr& sub : autonomy->subs()) {
             for (auto msg : sub->pop_msgs<sc::MessageBase>()) {
                 sub->accept(msg);
             }
@@ -210,10 +197,8 @@ bool AutonomyExecutor::step_autonomy(double t, double dt) {
         }
 
         if (show_shapes_) {
-            std::for_each(autonomy->shapes().begin(),
-                          autonomy->shapes().end(), [&](auto &s) {
-                              this->draw_shape(s);
-                          });
+            std::for_each(autonomy->shapes().begin(), autonomy->shapes().end(),
+                          [&](auto& s) { this->draw_shape(s); });
         }
     }
     return true;
@@ -238,5 +223,5 @@ bool AutonomyExecutor::call_init(std::string autonomy_name, sc::AutonomyPtr auto
     autonomy->init(config_parse.params());
     return true;
 }
-} // namespace autonomy
-} // namespace scrimmage
+}  // namespace autonomy
+}  // namespace scrimmage
diff --git a/src/plugins/autonomy/AvoidEntityMS/AvoidEntityMS.cpp b/src/plugins/autonomy/AvoidEntityMS/AvoidEntityMS.cpp
index 909e9563e0..6b2d59ca69 100644
--- a/src/plugins/autonomy/AvoidEntityMS/AvoidEntityMS.cpp
+++ b/src/plugins/autonomy/AvoidEntityMS/AvoidEntityMS.cpp
@@ -30,15 +30,14 @@
  *
  */
 
-#include <scrimmage/plugins/autonomy/AvoidEntityMS/AvoidEntityMS.h>
-
-#include <scrimmage/plugin_manager/RegisterPlugin.h>
-#include <scrimmage/entity/Entity.h>
 #include <scrimmage/common/Shape.h>
+#include <scrimmage/entity/Entity.h>
 #include <scrimmage/math/State.h>
 #include <scrimmage/parse/ParseUtils.h>
-#include <scrimmage/proto/Shape.pb.h>
+#include <scrimmage/plugin_manager/RegisterPlugin.h>
+#include <scrimmage/plugins/autonomy/AvoidEntityMS/AvoidEntityMS.h>
 #include <scrimmage/proto/ProtoConversions.h>
+#include <scrimmage/proto/Shape.pb.h>
 
 #include <iostream>
 #include <limits>
@@ -48,20 +47,18 @@ using std::endl;
 
 namespace sc = scrimmage;
 
-REGISTER_PLUGIN(scrimmage::Autonomy,
-                scrimmage::autonomy::motor_schemas::AvoidEntityMS,
+REGISTER_PLUGIN(scrimmage::Autonomy, scrimmage::autonomy::motor_schemas::AvoidEntityMS,
                 AvoidEntityMS_plugin)
 
 namespace scrimmage {
 namespace autonomy {
 namespace motor_schemas {
 
-AvoidEntityMS::AvoidEntityMS() : sphere_of_influence_(10.0),
-                                 minimum_range_(5.0),
-                                 avoid_non_team_(true) {
+AvoidEntityMS::AvoidEntityMS()
+    : sphere_of_influence_(10.0), minimum_range_(5.0), avoid_non_team_(true) {
 }
 
-void AvoidEntityMS::init(std::map<std::string, std::string> &params) {
+void AvoidEntityMS::init(std::map<std::string, std::string>& params) {
     sphere_of_influence_ = sc::get<double>("sphere_of_influence", params, 10);
     minimum_range_ = sc::get<double>("minimum_range", params, 5);
     avoid_non_team_ = sc::get<bool>("avoid_non_team", params, true);
@@ -70,16 +67,14 @@ void AvoidEntityMS::init(std::map<std::string, std::string> &params) {
     configure_contacts(params);
 }
 
-void AvoidEntityMS::avoidance_vectors(ContactMap &contacts,
-                                      std::vector<Eigen::Vector3d> &O_vecs) {
+void AvoidEntityMS::avoidance_vectors(ContactMap& contacts, std::vector<Eigen::Vector3d>& O_vecs) {
     for (auto it = contacts.begin(); it != contacts.end(); it++) {
         // Ignore own position / id
         if (it->second.id().id() == parent_->id().id()) {
             continue;
         }
 
-        if (!avoid_non_team_ &&
-            it->second.id().team_id() != parent_->id().team_id()) {
+        if (!avoid_non_team_ && it->second.id().team_id() != parent_->id().team_id()) {
             continue;
         }
 
@@ -91,8 +86,7 @@ void AvoidEntityMS::avoidance_vectors(ContactMap &contacts,
         if (dist > sphere_of_influence_) {
             O_mag = 0;
         } else if (minimum_range_ < dist && dist <= sphere_of_influence_) {
-            O_mag = (sphere_of_influence_ - dist) /
-                (sphere_of_influence_ - minimum_range_);
+            O_mag = (sphere_of_influence_ - dist) / (sphere_of_influence_ - minimum_range_);
         } else if (dist <= minimum_range_) {
             O_mag = 1e10;
         }
@@ -114,10 +108,10 @@ bool AvoidEntityMS::step_autonomy(double t, double dt) {
     }
 
     // Normalize each repulsion vector and sum
-    desired_vector_ <<  0, 0, 0;
+    desired_vector_ << 0, 0, 0;
     for (auto it = O_vecs.begin(); it != O_vecs.end(); it++) {
         if (it->hasNaN()) {
-            continue; // ignore misbehaved vectors
+            continue;  // ignore misbehaved vectors
         }
         desired_vector_ += *it;
     }
@@ -127,17 +121,16 @@ bool AvoidEntityMS::step_autonomy(double t, double dt) {
     if (show_shapes_) {
         // Draw the sphere of influence
         if (circle_shape_ == nullptr) {
-            circle_shape_ = sc::shape::make_circle(
-                state_->pos(), state_->quat(), sphere_of_influence_,
-                Eigen::Vector3d(0, 255, 0), 0.2);
+            circle_shape_ =
+                sc::shape::make_circle(state_->pos(), state_->quat(), sphere_of_influence_,
+                                       Eigen::Vector3d(0, 255, 0), 0.2);
         }
         sc::set(circle_shape_->mutable_circle()->mutable_center(), state_->pos());
         draw_shape(circle_shape_);
 
         if (line_shape_ == nullptr) {
-            line_shape_ = sc::shape::make_line(
-                state_->pos(), desired_vector_ + state_->pos(),
-                Eigen::Vector3d(0, 0, 0), 0.75);
+            line_shape_ = sc::shape::make_line(state_->pos(), desired_vector_ + state_->pos(),
+                                               Eigen::Vector3d(0, 0, 0), 0.75);
         }
         sc::set(line_shape_->mutable_line()->mutable_start(), state_->pos());
         sc::set(line_shape_->mutable_line()->mutable_end(), desired_vector_ + state_->pos());
@@ -146,6 +139,6 @@ bool AvoidEntityMS::step_autonomy(double t, double dt) {
 
     return true;
 }
-} // namespace motor_schemas
-} // namespace autonomy
-} // namespace scrimmage
+}  // namespace motor_schemas
+}  // namespace autonomy
+}  // namespace scrimmage
diff --git a/src/plugins/autonomy/AvoidWalls/AvoidWalls.cpp b/src/plugins/autonomy/AvoidWalls/AvoidWalls.cpp
index 567f1f1469..2735930114 100644
--- a/src/plugins/autonomy/AvoidWalls/AvoidWalls.cpp
+++ b/src/plugins/autonomy/AvoidWalls/AvoidWalls.cpp
@@ -30,11 +30,10 @@
  *
  */
 
-
-#include <scrimmage/plugin_manager/RegisterPlugin.h>
 #include <scrimmage/entity/Entity.h>
 #include <scrimmage/math/State.h>
 #include <scrimmage/parse/ParseUtils.h>
+#include <scrimmage/plugin_manager/RegisterPlugin.h>
 #include <scrimmage/plugins/autonomy/AvoidWalls/AvoidWalls.h>
 #include <scrimmage/plugins/sensor/RayTrace/RayTrace.h>
 #include <scrimmage/pubsub/Subscriber.h>
@@ -44,11 +43,11 @@ REGISTER_PLUGIN(scrimmage::Autonomy, scrimmage::autonomy::AvoidWalls, AvoidWalls
 namespace scrimmage {
 namespace autonomy {
 
-void AvoidWalls::init(std::map<std::string, std::string> &params) {
+void AvoidWalls::init(std::map<std::string, std::string>& params) {
     double initial_speed = get<double>("initial_speed", params, 21);
     avoid_distance_ = get<double>("avoid_distance", params, 20);
 
-    auto pc_cb = [&] (scrimmage::MessagePtr<sensor::RayTrace::PointCloud> msg) {
+    auto pc_cb = [&](scrimmage::MessagePtr<sensor::RayTrace::PointCloud> msg) {
         point_cloud_ = msg->data;
     };
     subscribe<sensor::RayTrace::PointCloud>("LocalNetwork", "RayTrace/pointcloud", pc_cb);
@@ -64,7 +63,7 @@ bool AvoidWalls::step_autonomy(double t, double dt) {
     // Find closest point and move away from it
     bool all_close_points = true;
     std::list<Eigen::Vector3d> points;
-    for (sensor::RayTrace::PCPoint &p : point_cloud_.points) {
+    for (sensor::RayTrace::PCPoint& p : point_cloud_.points) {
         if (p.point.norm() < avoid_distance_) {
             points.push_back(p.point);
         } else {
@@ -74,7 +73,7 @@ bool AvoidWalls::step_autonomy(double t, double dt) {
 
     if (!points.empty()) {
         std::vector<Eigen::Vector3d> O_vecs;
-        for (Eigen::Vector3d &p : points) {
+        for (Eigen::Vector3d& p : points) {
             // Transform the point to global coordinate space
             double dist = p.norm();
             p = state_->quat().rotate(p);
@@ -84,7 +83,7 @@ bool AvoidWalls::step_autonomy(double t, double dt) {
             Eigen::Vector3d diff = p - state_->pos();
             double O_mag = avoid_distance_ - dist;
 
-            Eigen::Vector3d O_dir = - O_mag * diff.normalized();
+            Eigen::Vector3d O_dir = -O_mag * diff.normalized();
             O_vecs.push_back(O_dir);
         }
 
@@ -92,7 +91,7 @@ bool AvoidWalls::step_autonomy(double t, double dt) {
         Eigen::Vector3d O_vec(0, 0, 0);
         for (auto it = O_vecs.begin(); it != O_vecs.end(); it++) {
             if (it->hasNaN()) {
-                continue; // ignore misbehaved vectors
+                continue;  // ignore misbehaved vectors
             }
             O_vec += *it;
         }
@@ -116,5 +115,5 @@ bool AvoidWalls::step_autonomy(double t, double dt) {
 
     return true;
 }
-} // namespace autonomy
-} // namespace scrimmage
+}  // namespace autonomy
+}  // namespace scrimmage
diff --git a/src/plugins/autonomy/Boids/Boids.cpp b/src/plugins/autonomy/Boids/Boids.cpp
index fe58c6ca85..d18eee5fd7 100644
--- a/src/plugins/autonomy/Boids/Boids.cpp
+++ b/src/plugins/autonomy/Boids/Boids.cpp
@@ -33,13 +33,13 @@
 #include <scrimmage/common/RTree.h>
 #include <scrimmage/common/Utilities.h>
 #include <scrimmage/entity/Entity.h>
-#include <scrimmage/math/State.h>
 #include <scrimmage/math/Angles.h>
+#include <scrimmage/math/State.h>
 #include <scrimmage/parse/ParseUtils.h>
 #include <scrimmage/plugin_manager/RegisterPlugin.h>
 #include <scrimmage/plugins/autonomy/Boids/Boids.h>
-#include <scrimmage/proto/Shape.pb.h>
 #include <scrimmage/proto/ProtoConversions.h>
+#include <scrimmage/proto/Shape.pb.h>
 
 #include <vector>
 
@@ -50,8 +50,7 @@ namespace sc = scrimmage;
 namespace scrimmage {
 namespace autonomy {
 
-void Boids::init(std::map<std::string, std::string> &params) {
-
+void Boids::init(std::map<std::string, std::string>& params) {
     show_shapes_ = get("show_shapes", params, false);
     max_speed_ = get<double>("max_speed", params, 21);
 
@@ -90,9 +89,11 @@ void Boids::init(std::map<std::string, std::string> &params) {
     io_turn_rate_idx_ = vars_.declare(VariableIO::Type::turn_rate, VariableIO::Direction::Out);
     io_pitch_rate_idx_ = vars_.declare(VariableIO::Type::pitch_rate, VariableIO::Direction::Out);
 
-    io_desired_speed_idx_ = vars_.declare(VariableIO::Type::desired_speed, VariableIO::Direction::Out);
+    io_desired_speed_idx_ =
+        vars_.declare(VariableIO::Type::desired_speed, VariableIO::Direction::Out);
     io_heading_idx_ = vars_.declare(VariableIO::Type::desired_heading, VariableIO::Direction::Out);
-    io_altitude_idx_ = vars_.declare(VariableIO::Type::desired_altitude, VariableIO::Direction::Out);
+    io_altitude_idx_ =
+        vars_.declare(VariableIO::Type::desired_altitude, VariableIO::Direction::Out);
 }
 
 bool Boids::step_autonomy(double t, double dt) {
@@ -103,7 +104,6 @@ bool Boids::step_autonomy(double t, double dt) {
     // Remove neighbors that are not within field of view
     for (auto it = rtree_neighbors.begin(); it != rtree_neighbors.end();
          /* no inc */) {
-
         // Ignore own position / id
         if (it->id() == parent_->id().id()) {
             it = rtree_neighbors.erase(it);
@@ -146,14 +146,13 @@ bool Boids::step_autonomy(double t, double dt) {
         if (dist > sphere_of_influence_) {
             O_mag = 0;
         } else if (min_range < dist && dist <= sphere_of_influence_) {
-            O_mag = (sphere_of_influence_ - dist) /
-                (sphere_of_influence_ - min_range);
+            O_mag = (sphere_of_influence_ - dist) / (sphere_of_influence_ - min_range);
         } else if (dist <= min_range) {
             O_mag = 1e10;
         }
 
         // Calculate repulsion vector
-        Eigen::Vector3d O_dir = - O_mag * diff.normalized();
+        Eigen::Vector3d O_dir = -O_mag * diff.normalized();
         if (is_team) {
             O_team_vecs.push_back(O_dir);
         } else {
@@ -177,7 +176,7 @@ bool Boids::step_autonomy(double t, double dt) {
     }
 
     // Make sure alignment vector is well-behaved
-    align = align_vec; // TODO
+    align = align_vec;  // TODO
     Eigen::Vector3d v_align_normed = align.normalized();
     double v_align_norm = align.norm();
     if (v_align_normed.hasNaN()) {
@@ -189,7 +188,7 @@ bool Boids::step_autonomy(double t, double dt) {
     Eigen::Vector3d O_team_vec(0, 0, 0);
     for (Eigen::Vector3d v : O_team_vecs) {
         if (v.hasNaN()) {
-            continue; // ignore misbehaved vectors
+            continue;  // ignore misbehaved vectors
         }
         O_team_vec += v;
     }
@@ -198,7 +197,7 @@ bool Boids::step_autonomy(double t, double dt) {
     Eigen::Vector3d O_nonteam_vec(0, 0, 0);
     for (Eigen::Vector3d v : O_nonteam_vecs) {
         if (v.hasNaN()) {
-            continue; // ignore misbehaved vectors
+            continue;  // ignore misbehaved vectors
         }
         O_nonteam_vec += v;
     }
@@ -210,19 +209,17 @@ bool Boids::step_autonomy(double t, double dt) {
     Eigen::Vector3d v_centroid_w_gain = (centroid - state_->pos()).normalized() * w_centroid_;
     Eigen::Vector3d v_align_w_gain = v_align_normed * w_align_;
 
-    double sum_norms = v_goal_w_gain.norm() + O_team_vec_w_gain.norm() +
-        O_nonteam_vec_w_gain.norm() + v_centroid_w_gain.norm() +
-        v_align_norm;
+    double sum_norms = v_goal_w_gain.norm() + O_team_vec_w_gain.norm() + O_nonteam_vec_w_gain.norm()
+                       + v_centroid_w_gain.norm() + v_align_norm;
 
-    Eigen::Vector3d v_sum = (v_goal_w_gain + O_team_vec_w_gain +
-                             O_nonteam_vec_w_gain + v_centroid_w_gain +
-                             v_align_w_gain) / sum_norms;
+    Eigen::Vector3d v_sum = (v_goal_w_gain + O_team_vec_w_gain + O_nonteam_vec_w_gain
+                             + v_centroid_w_gain + v_align_w_gain)
+                            / sum_norms;
 
     // Scale velocity to max speed:
     Eigen::Vector3d vel_result = v_sum * max_speed_;
 
     if (rtree_neighbors.size() > 0) {
-
         velocity_controller(vel_result);
 
         if (show_shapes_) {
@@ -247,8 +244,7 @@ bool Boids::step_autonomy(double t, double dt) {
     return true;
 }
 
-
-void Boids::velocity_controller(Eigen::Vector3d &v) {
+void Boids::velocity_controller(Eigen::Vector3d& v) {
     // Convert to spherical coordinates:
     double desired_heading = atan2(v(1), v(0));
     double desired_pitch = atan2(v(2), v.head<2>().norm());
@@ -271,5 +267,5 @@ void Boids::velocity_controller(Eigen::Vector3d &v) {
     vars_.output(io_vel_y_idx_, v(1));
     vars_.output(io_vel_z_idx_, v(2));
 }
-} // namespace autonomy
-} // namespace scrimmage
+}  // namespace autonomy
+}  // namespace scrimmage
diff --git a/src/plugins/autonomy/BoundaryDefense/BoundaryDefense.cpp b/src/plugins/autonomy/BoundaryDefense/BoundaryDefense.cpp
index 4dbd3a4d7e..beee73dec2 100644
--- a/src/plugins/autonomy/BoundaryDefense/BoundaryDefense.cpp
+++ b/src/plugins/autonomy/BoundaryDefense/BoundaryDefense.cpp
@@ -30,35 +30,31 @@
  *
  */
 
-#include <scrimmage/plugins/autonomy/BoundaryDefense/BoundaryDefense.h>
-#include <scrimmage/plugin_manager/RegisterPlugin.h>
+#include <scrimmage/common/RTree.h>
+#include <scrimmage/common/Time.h>
+#include <scrimmage/common/Waypoint.h>
 #include <scrimmage/entity/Entity.h>
 #include <scrimmage/math/State.h>
 #include <scrimmage/parse/ParseUtils.h>
-#include <scrimmage/common/RTree.h>
+#include <scrimmage/plugin_manager/RegisterPlugin.h>
+#include <scrimmage/plugins/autonomy/BoundaryDefense/BoundaryDefense.h>
+#include <scrimmage/plugins/autonomy/WaypointGenerator/WaypointList.h>
+#include <scrimmage/plugins/interaction/Boundary/Boundary.h>
+#include <scrimmage/plugins/interaction/Boundary/BoundaryBase.h>
 #include <scrimmage/pubsub/Publisher.h>
 #include <scrimmage/pubsub/Subscriber.h>
-#include <scrimmage/common/Time.h>
-
-#include <scrimmage/plugins/interaction/Boundary/BoundaryBase.h>
-#include <scrimmage/plugins/interaction/Boundary/Boundary.h>
-#include <scrimmage/common/Waypoint.h>
-#include <scrimmage/plugins/autonomy/WaypointGenerator/WaypointList.h>
 
+#include <GeographicLib/LocalCartesian.hpp>
 #include <iostream>
 #include <limits>
 
-#include <GeographicLib/LocalCartesian.hpp>
-
 using std::cout;
 using std::endl;
 
 namespace sc = scrimmage;
 namespace sci = scrimmage::interaction;
 
-REGISTER_PLUGIN(scrimmage::Autonomy,
-                scrimmage::autonomy::BoundaryDefense,
-                BoundaryDefense_plugin)
+REGISTER_PLUGIN(scrimmage::Autonomy, scrimmage::autonomy::BoundaryDefense, BoundaryDefense_plugin)
 
 namespace scrimmage {
 namespace autonomy {
@@ -66,10 +62,10 @@ namespace autonomy {
 BoundaryDefense::BoundaryDefense() {
 }
 
-void BoundaryDefense::init(std::map<std::string, std::string> &params) {
+void BoundaryDefense::init(std::map<std::string, std::string>& params) {
     boundary_id_ = sc::get<int>("boundary_id", params, boundary_id_);
 
-    auto callback = [&] (scrimmage::MessagePtr<sp::Shape> msg) {
+    auto callback = [&](scrimmage::MessagePtr<sp::Shape> msg) {
         std::shared_ptr<sci::BoundaryBase> boundary = sci::Boundary::make_boundary(msg->data);
         boundaries_[msg->data.id().id()] = std::make_pair(msg->data, boundary);
     };
@@ -91,8 +87,8 @@ bool BoundaryDefense::step_autonomy(double t, double dt) {
     // boundary's team.
     double min_dist = std::numeric_limits<double>::infinity();
     sc::StatePtr closest = nullptr;
-    for (auto &kv : *contacts_) {
-        sc::Contact &cnt = kv.second;
+    for (auto& kv : *contacts_) {
+        sc::Contact& cnt = kv.second;
 
         // Ignore same team
         if (cnt.id().team_id() == std::get<0>(it->second).id().team_id()) {
@@ -101,8 +97,7 @@ bool BoundaryDefense::step_autonomy(double t, double dt) {
 
         // Is the entity within the boundary?
         if (std::get<1>(it->second)->contains(cnt.state()->pos())) {
-            if (closest == nullptr ||
-                (state_->pos() - closest->pos()).norm() < min_dist) {
+            if (closest == nullptr || (state_->pos() - closest->pos()).norm() < min_dist) {
                 closest = cnt.state();
                 min_dist = (state_->pos() - closest->pos()).norm();
             }
@@ -129,5 +124,5 @@ bool BoundaryDefense::step_autonomy(double t, double dt) {
 
     return true;
 }
-} // namespace autonomy
-} // namespace scrimmage
+}  // namespace autonomy
+}  // namespace scrimmage
diff --git a/src/plugins/autonomy/CommandStringRelay/CommandStringRelay.cpp b/src/plugins/autonomy/CommandStringRelay/CommandStringRelay.cpp
index 998582a3c2..4f3e89c7a4 100644
--- a/src/plugins/autonomy/CommandStringRelay/CommandStringRelay.cpp
+++ b/src/plugins/autonomy/CommandStringRelay/CommandStringRelay.cpp
@@ -30,28 +30,25 @@
  *
  */
 
-#include <scrimmage/plugins/autonomy/CommandStringRelay/CommandStringRelay.h>
-
-#include <scrimmage/plugin_manager/RegisterPlugin.h>
 #include <scrimmage/entity/Entity.h>
 #include <scrimmage/math/State.h>
-#include <scrimmage/parse/ParseUtils.h>
 #include <scrimmage/msgs/Command.pb.h>
+#include <scrimmage/parse/ParseUtils.h>
+#include <scrimmage/plugin_manager/RegisterPlugin.h>
+#include <scrimmage/plugins/autonomy/CommandStringRelay/CommandStringRelay.h>
 #include <scrimmage/pubsub/Publisher.h>
 
+#include <boost/algorithm/string/classification.hpp>
+#include <boost/algorithm/string/split.hpp>
 #include <iostream>
 #include <limits>
 
-#include <boost/algorithm/string/split.hpp>
-#include <boost/algorithm/string/classification.hpp>
-
 using std::cout;
 using std::endl;
 
 namespace sc = scrimmage;
 
-REGISTER_PLUGIN(scrimmage::Autonomy,
-                scrimmage::autonomy::CommandStringRelay,
+REGISTER_PLUGIN(scrimmage::Autonomy, scrimmage::autonomy::CommandStringRelay,
                 CommandStringRelay_plugin)
 
 namespace scrimmage {
@@ -60,7 +57,7 @@ namespace autonomy {
 CommandStringRelay::CommandStringRelay() {
 }
 
-void CommandStringRelay::init(std::map<std::string, std::string> &params) {
+void CommandStringRelay::init(std::map<std::string, std::string>& params) {
     std::string relays_str = sc::get<std::string>("relays", params, "");
     std::vector<std::vector<std::string>> vecs_of_vecs;
     sc::get_vec_of_vecs(relays_str, vecs_of_vecs, " ");
@@ -76,7 +73,7 @@ void CommandStringRelay::init(std::map<std::string, std::string> &params) {
         std::string original_topic_str = "/" + std::to_string(parent_->id().id()) + "/" + topic;
         pubs_[topic] = advertise(relay_to_network, topic);
 
-        auto cb = [&] (scrimmage::MessagePtr<scrimmage_msgs::CommandString> msg) {
+        auto cb = [&](scrimmage::MessagePtr<scrimmage_msgs::CommandString> msg) {
             std::vector<std::string> tokens;
             boost::split(tokens, msg->data.topic(), boost::is_any_of("/"));
 
@@ -101,5 +98,5 @@ void CommandStringRelay::init(std::map<std::string, std::string> &params) {
 bool CommandStringRelay::step_autonomy(double t, double dt) {
     return true;
 }
-} // namespace autonomy
-} // namespace scrimmage
+}  // namespace autonomy
+}  // namespace scrimmage
diff --git a/src/plugins/autonomy/Control3D/Control3D.cpp b/src/plugins/autonomy/Control3D/Control3D.cpp
index ecf3a6528d..81c4ba4b3c 100644
--- a/src/plugins/autonomy/Control3D/Control3D.cpp
+++ b/src/plugins/autonomy/Control3D/Control3D.cpp
@@ -30,10 +30,9 @@
  *
  */
 
-#include <scrimmage/plugin_manager/RegisterPlugin.h>
 #include <scrimmage/entity/Entity.h>
 #include <scrimmage/math/State.h>
-
+#include <scrimmage/plugin_manager/RegisterPlugin.h>
 #include <scrimmage/plugins/autonomy/Control3D/Control3D.h>
 
 namespace sc = scrimmage;
@@ -43,16 +42,17 @@ REGISTER_PLUGIN(scrimmage::Autonomy, scrimmage::autonomy::Control3D, Control3D_p
 namespace scrimmage {
 namespace autonomy {
 
-Control3D::Control3D() {}
+Control3D::Control3D() {
+}
 
-void Control3D::init(std::map<std::string, std::string> &params) {
-    desired_state_->vel() = Eigen::Vector3d::UnitX()*21;
+void Control3D::init(std::map<std::string, std::string>& params) {
+    desired_state_->vel() = Eigen::Vector3d::UnitX() * 21;
     desired_state_->quat().set(0, 0, state_->quat().yaw());
-    desired_state_->pos() = Eigen::Vector3d::UnitZ()*state_->pos()(2);
+    desired_state_->pos() = Eigen::Vector3d::UnitZ() * state_->pos()(2);
 }
 
 bool Control3D::step_autonomy(double t, double dt) {
     return true;
 }
-} // namespace autonomy
-} // namespace scrimmage
+}  // namespace autonomy
+}  // namespace scrimmage
diff --git a/src/plugins/autonomy/FlightGearMultiplayer/FlightGearMultiplayer.cpp b/src/plugins/autonomy/FlightGearMultiplayer/FlightGearMultiplayer.cpp
index 9c9aaadc4d..6d624a5bb0 100644
--- a/src/plugins/autonomy/FlightGearMultiplayer/FlightGearMultiplayer.cpp
+++ b/src/plugins/autonomy/FlightGearMultiplayer/FlightGearMultiplayer.cpp
@@ -30,61 +30,54 @@
  *
  */
 
-#include <scrimmage/plugins/autonomy/FlightGearMultiplayer/FlightGearMultiplayer.h>
-
-#include <scrimmage/plugin_manager/RegisterPlugin.h>
+#include <arpa/inet.h>
+#include <netdb.h>
+#include <netinet/in.h>
+#include <scrimmage/common/Time.h>
 #include <scrimmage/entity/Entity.h>
 #include <scrimmage/math/State.h>
 #include <scrimmage/parse/ParseUtils.h>
-#include <scrimmage/common/Time.h>
-
+#include <scrimmage/plugin_manager/RegisterPlugin.h>
+#include <scrimmage/plugins/autonomy/FlightGearMultiplayer/FlightGearMultiplayer.h>
 #include <string.h>
-#include <sys/types.h>
 #include <sys/socket.h>
-#include <netinet/in.h>
-#include <arpa/inet.h>
+#include <sys/types.h>
 #include <unistd.h>
-#include <netdb.h>
-
-#include <iostream>
-#include <limits>
 
+#include <GeographicLib/LocalCartesian.hpp>
 #include <flightgear/MultiPlayer/mpmessages.hxx>
 #include <flightgear/MultiPlayer/tiny_xdr.hxx>
-
-#include <GeographicLib/LocalCartesian.hpp>
+#include <iostream>
+#include <limits>
 
 using std::cout;
 using std::endl;
 
 namespace sc = scrimmage;
 
-REGISTER_PLUGIN(scrimmage::Autonomy,
-                scrimmage::autonomy::FlightGearMultiplayer,
+REGISTER_PLUGIN(scrimmage::Autonomy, scrimmage::autonomy::FlightGearMultiplayer,
                 FlightGearMultiplayer_plugin)
 
 namespace scrimmage {
 namespace autonomy {
 
-FlightGearMultiplayer::FlightGearMultiplayer() :
-    callsign_("scrimmage"),
-    data_socket_(std::make_shared<netSocket>()),
-    aircraft_model_(std::string("Aircraft/c172p/Models/c172p.xml")),
-    earth_(std::make_shared<GeographicLib::Geocentric>(
-               GeographicLib::Constants::WGS84_a(),
-               GeographicLib::Constants::WGS84_f())),
-    angles_to_jsbsim_(0, Angles::Type::EUCLIDEAN, Angles::Type::GPS) {
+FlightGearMultiplayer::FlightGearMultiplayer()
+    : callsign_("scrimmage"),
+      data_socket_(std::make_shared<netSocket>()),
+      aircraft_model_(std::string("Aircraft/c172p/Models/c172p.xml")),
+      earth_(std::make_shared<GeographicLib::Geocentric>(GeographicLib::Constants::WGS84_a(),
+                                                         GeographicLib::Constants::WGS84_f())),
+      angles_to_jsbsim_(0, Angles::Type::EUCLIDEAN, Angles::Type::GPS) {
 }
 
-void FlightGearMultiplayer::init(std::map<std::string, std::string> &params) {
-
+void FlightGearMultiplayer::init(std::map<std::string, std::string>& params) {
     std::string server_ip = sc::get<std::string>("server_ip", params, "127.0.0.1");
     int server_port = sc::get<int>("server_port", params, 5000);
     callsign_ = sc::get<std::string>("callsign", params, "scrimmage");
     aircraft_model_ = sc::get<std::string>("aircraft_model", params, aircraft_model_);
 
     net_address_.set(server_ip.c_str(), server_port);
-    if (data_socket_->open(false)  == 0) { // Open UDP socket
+    if (data_socket_->open(false) == 0) {  // Open UDP socket
         std::cout << "Failed to open connection to: " << endl;
         cout << "Server IP: " << net_address_.getIP() << endl;
         cout << "Server Port: " << net_address_.getPort() << endl;
@@ -110,9 +103,9 @@ void FlightGearMultiplayer::init(std::map<std::string, std::string> &params) {
     pos_msg_.angularVel[1] = XDR_encode<float>(0);
     pos_msg_.angularVel[2] = XDR_encode<float>(0);
 
-    pos_msg_.linearAccel[0]= XDR_encode<float>(0);
-    pos_msg_.linearAccel[1]= XDR_encode<float>(0);
-    pos_msg_.linearAccel[2]= XDR_encode<float>(0);
+    pos_msg_.linearAccel[0] = XDR_encode<float>(0);
+    pos_msg_.linearAccel[1] = XDR_encode<float>(0);
+    pos_msg_.linearAccel[2] = XDR_encode<float>(0);
 
     pos_msg_.angularAccel[0] = XDR_encode<float>(0);
     pos_msg_.angularAccel[1] = XDR_encode<float>(0);
@@ -129,7 +122,8 @@ scrimmage::Quaternion FlightGearMultiplayer::fromLonLatRad(float lon, float lat)
     // Eigen::Vector3d::UnitX()); Eigen::Quaterniond rot2 = R_a * R_b;
 
     float zd2 = static_cast<float>(0.5) * lon;
-    float yd2 = static_cast<float>(-0.25) * static_cast<float>(M_PI) - static_cast<float>(0.5) * lat;
+    float yd2 =
+        static_cast<float>(-0.25) * static_cast<float>(M_PI) - static_cast<float>(0.5) * lat;
     float Szd2 = sin(zd2);
     float Syd2 = sin(yd2);
     float Czd2 = cos(zd2);
@@ -145,9 +139,8 @@ bool FlightGearMultiplayer::step_autonomy(double t, double dt) {
 
     // Compute ECEF coordinates from x/y/z lat/lon/alt
     double lat, lon, alt;
-    parent_->projection()->Reverse(state_->pos()(0), state_->pos()(1),
-                                   state_->pos()(2),
-                                   lat, lon, alt);
+    parent_->projection()->Reverse(state_->pos()(0), state_->pos()(1), state_->pos()(2), lat, lon,
+                                   alt);
     double ECEF_X, ECEF_Y, ECEF_Z;
     earth_->Forward(lat, lon, alt, ECEF_X, ECEF_Y, ECEF_Z);
 
@@ -161,8 +154,8 @@ bool FlightGearMultiplayer::step_autonomy(double t, double dt) {
                               sc::Angles::deg2rad(angles_to_jsbsim_.angle()));
 
     // Convert local quaternion to ECEF coordinate
-    scrimmage::Quaternion rot = this->fromLonLatRad(sc::Angles::deg2rad(lon),
-                                                    sc::Angles::deg2rad(lat));
+    scrimmage::Quaternion rot =
+        this->fromLonLatRad(sc::Angles::deg2rad(lon), sc::Angles::deg2rad(lat));
 
     // The FlightGear Multiplayer packet expects the orientation to be in
     // angle-axis form, where the axis magnitude is the rotation angle.
@@ -174,14 +167,14 @@ bool FlightGearMultiplayer::step_autonomy(double t, double dt) {
     pos_msg_.orientation[2] = XDR_encode<float>(ang_axis_mod(2));
 
     // Copy header and position data structs into memory buffer
-    char * msg = new char[msg_size_];
+    char* msg = new char[msg_size_];
     std::memcpy(msg, &header_msg_, sizeof(T_MsgHdr));
-    std::memcpy(msg+sizeof(T_MsgHdr), &pos_msg_, sizeof(T_PositionMsg));
+    std::memcpy(msg + sizeof(T_MsgHdr), &pos_msg_, sizeof(T_PositionMsg));
 
     data_socket_->sendto(msg, msg_size_, 0, &net_address_);
     delete[] msg;
 
     return true;
 }
-} // namespace autonomy
-} // namespace scrimmage
+}  // namespace autonomy
+}  // namespace scrimmage
diff --git a/src/plugins/autonomy/GoToWaypoint/GoToWaypoint.cpp b/src/plugins/autonomy/GoToWaypoint/GoToWaypoint.cpp
index d2a7d8a4f8..9b819bb69f 100644
--- a/src/plugins/autonomy/GoToWaypoint/GoToWaypoint.cpp
+++ b/src/plugins/autonomy/GoToWaypoint/GoToWaypoint.cpp
@@ -30,32 +30,29 @@
  *
  */
 
-#include <scrimmage/plugins/autonomy/GoToWaypoint/GoToWaypoint.h>
-
-#include <scrimmage/plugin_manager/RegisterPlugin.h>
 #include <scrimmage/entity/Entity.h>
+#include <scrimmage/math/Angles.h>
 #include <scrimmage/math/State.h>
 #include <scrimmage/parse/ParseUtils.h>
-#include <scrimmage/math/Angles.h>
+#include <scrimmage/plugin_manager/RegisterPlugin.h>
+#include <scrimmage/plugins/autonomy/GoToWaypoint/GoToWaypoint.h>
 #include <scrimmage/pubsub/Publisher.h>
-#include <iostream>
-#include <limits>
 
 #include <GeographicLib/LocalCartesian.hpp>
+#include <iostream>
+#include <limits>
 
 using std::cout;
 using std::endl;
 
 namespace sc = scrimmage;
 
-REGISTER_PLUGIN(scrimmage::Autonomy,
-                scrimmage::autonomy::GoToWaypoint,
-                GoToWaypoint_plugin)
+REGISTER_PLUGIN(scrimmage::Autonomy, scrimmage::autonomy::GoToWaypoint, GoToWaypoint_plugin)
 
 namespace scrimmage {
 namespace autonomy {
 
-void GoToWaypoint::init(std::map<std::string, std::string> &params) {
+void GoToWaypoint::init(std::map<std::string, std::string>& params) {
     waypoint_status_ = str2container(params["waypoint"], " ", waypoint_);
 
     waypoint_list_pub_ = advertise("LocalNetwork", "WaypointList");
@@ -63,19 +60,17 @@ void GoToWaypoint::init(std::map<std::string, std::string> &params) {
 
 bool GoToWaypoint::step_autonomy(double t, double dt) {
     if (waypoint_status_) {
-
         sc::State ns = *(parent_->state());
 
-        double lat = (waypoint_[2] == "X") ? ns.pos()(0): stod(waypoint_[2]);
-        double lon = (waypoint_[3] == "Y") ? ns.pos()(1): stod(waypoint_[3]);
-        double alt = (waypoint_[4] == "Z") ? ns.pos()(2): stod(waypoint_[4]);
+        double lat = (waypoint_[2] == "X") ? ns.pos()(0) : stod(waypoint_[2]);
+        double lon = (waypoint_[3] == "Y") ? ns.pos()(1) : stod(waypoint_[3]);
+        double alt = (waypoint_[4] == "Z") ? ns.pos()(2) : stod(waypoint_[4]);
 
         std::string type = waypoint_[0];
         if (type != "XYZ" && type != "GPS") {
-            std::cout<< "Invalid waypoint type: " << type << std::endl;
+            std::cout << "Invalid waypoint type: " << type << std::endl;
         } else if (type == "XYZ") {
-            parent_->projection()->Reverse(lat, lon, alt,
-                                               lat, lon, alt);
+            parent_->projection()->Reverse(lat, lon, alt, lat, lon, alt);
         }
 
         Waypoint wp(lat, lon, alt);
@@ -118,5 +113,5 @@ void GoToWaypoint::publish_waypoint(double t) {
     }
 }
 
-} // namespace autonomy
-} // namespace scrimmage
+}  // namespace autonomy
+}  // namespace scrimmage
diff --git a/src/plugins/autonomy/GraphvizFSM/GraphvizFSM.cpp b/src/plugins/autonomy/GraphvizFSM/GraphvizFSM.cpp
index 0e6555a9f6..33d1b32c8e 100644
--- a/src/plugins/autonomy/GraphvizFSM/GraphvizFSM.cpp
+++ b/src/plugins/autonomy/GraphvizFSM/GraphvizFSM.cpp
@@ -30,27 +30,23 @@
  *
  */
 
-#include <scrimmage/plugins/autonomy/GraphvizFSM/GraphvizFSM.h>
-
-#include <scrimmage/plugin_manager/RegisterPlugin.h>
 #include <scrimmage/entity/Entity.h>
 #include <scrimmage/math/State.h>
 #include <scrimmage/parse/ParseUtils.h>
+#include <scrimmage/plugin_manager/RegisterPlugin.h>
+#include <scrimmage/plugins/autonomy/GraphvizFSM/GraphvizFSM.h>
 #include <scrimmage/pubsub/Publisher.h>
 
+#include <boost/algorithm/string/replace.hpp>
 #include <iostream>
 #include <limits>
 
-#include <boost/algorithm/string/replace.hpp>
-
 using std::cout;
 using std::endl;
 
 namespace sc = scrimmage;
 
-REGISTER_PLUGIN(scrimmage::Autonomy,
-                scrimmage::autonomy::GraphvizFSM,
-                GraphvizFSM_plugin)
+REGISTER_PLUGIN(scrimmage::Autonomy, scrimmage::autonomy::GraphvizFSM, GraphvizFSM_plugin)
 
 namespace scrimmage {
 namespace autonomy {
@@ -58,7 +54,7 @@ namespace autonomy {
 GraphvizFSM::GraphvizFSM() {
 }
 
-void GraphvizFSM::init(std::map<std::string, std::string> &params) {
+void GraphvizFSM::init(std::map<std::string, std::string>& params) {
     print_current_state_ = sc::get<bool>("print_current_state", params, print_current_state_);
     std::string graph_str = sc::get<std::string>("graphviz_fsm", params, "");
 
@@ -68,7 +64,7 @@ void GraphvizFSM::init(std::map<std::string, std::string> &params) {
 
     boost::dynamic_properties dp(boost::ignore_other_properties);
 
-    dp.property("node_id", boost::get(&DotVertex::name,  fsm_graph_));
+    dp.property("node_id", boost::get(&DotVertex::name, fsm_graph_));
 
     label_ = get(label_t(), fsm_graph_);
     dp.property("label", label_);
@@ -111,14 +107,14 @@ void GraphvizFSM::init(std::map<std::string, std::string> &params) {
     state_pub_ = advertise(network_name, state_topic);
 
     // Subscribe to event topic
-    auto event_callback = [&] (scrimmage::MessagePtr<std::string> msg) {
+    auto event_callback = [&](scrimmage::MessagePtr<std::string> msg) {
         // If the event received is a trigger for the current state, transition
         // to the next state.
         typename boost::graph_traits<graph_t>::out_edge_iterator ei, ei_end;
-        for (boost::tie(ei, ei_end) = boost::out_edges(current_state_, fsm_graph_); ei != ei_end; ++ei) {
+        for (boost::tie(ei, ei_end) = boost::out_edges(current_state_, fsm_graph_); ei != ei_end;
+             ++ei) {
             if (label_(*ei) == msg->data) {
-                this->update_state_info(current_state_,
-                                        boost::target(*ei, fsm_graph_));
+                this->update_state_info(current_state_, boost::target(*ei, fsm_graph_));
             }
         }
     };
@@ -133,20 +129,18 @@ bool GraphvizFSM::step_autonomy(double t, double dt) {
     // state since this is just a designator for the "initial" state.
     if (boost::out_degree(current_state_, fsm_graph_) == 1) {
         typename boost::graph_traits<graph_t>::out_edge_iterator ei, ei_end;
-        for (boost::tie(ei, ei_end) = boost::out_edges(current_state_, fsm_graph_); ei != ei_end; ++ei) {
+        for (boost::tie(ei, ei_end) = boost::out_edges(current_state_, fsm_graph_); ei != ei_end;
+             ++ei) {
             if (label_(*ei) == std::string("")) {
-                this->update_state_info(current_state_,
-                                        boost::target(*ei, fsm_graph_));
+                this->update_state_info(current_state_, boost::target(*ei, fsm_graph_));
             }
         }
     }
     return true;
 }
 
-void GraphvizFSM::update_state_info(
-    boost::graph_traits<graph_t>::vertex_descriptor current_state,
-    boost::graph_traits<graph_t>::vertex_descriptor next_state) {
-
+void GraphvizFSM::update_state_info(boost::graph_traits<graph_t>::vertex_descriptor current_state,
+                                    boost::graph_traits<graph_t>::vertex_descriptor next_state) {
     current_state_ = next_state;
 
     if (print_current_state_) {
@@ -158,5 +152,5 @@ void GraphvizFSM::update_state_info(
     state_pub_->publish(msg);
 }
 
-} // namespace autonomy
-} // namespace scrimmage
+}  // namespace autonomy
+}  // namespace scrimmage
diff --git a/src/plugins/autonomy/JoystickAutonomy/JoystickAutonomy.cpp b/src/plugins/autonomy/JoystickAutonomy/JoystickAutonomy.cpp
index f382de5b0e..5b46a78a9b 100644
--- a/src/plugins/autonomy/JoystickAutonomy/JoystickAutonomy.cpp
+++ b/src/plugins/autonomy/JoystickAutonomy/JoystickAutonomy.cpp
@@ -30,12 +30,11 @@
  *
  */
 
-#include <scrimmage/plugins/autonomy/JoystickAutonomy/JoystickAutonomy.h>
-
-#include <scrimmage/plugin_manager/RegisterPlugin.h>
 #include <scrimmage/entity/Entity.h>
 #include <scrimmage/math/State.h>
 #include <scrimmage/parse/ParseUtils.h>
+#include <scrimmage/plugin_manager/RegisterPlugin.h>
+#include <scrimmage/plugins/autonomy/JoystickAutonomy/JoystickAutonomy.h>
 #include <scrimmage/pubsub/PubSub.h>
 
 #include <iostream>
@@ -46,20 +45,17 @@ using std::endl;
 
 namespace sc = scrimmage;
 
-REGISTER_PLUGIN(scrimmage::Autonomy,
-                scrimmage::autonomy::JoystickAutonomy,
-                JoystickAutonomy_plugin)
+REGISTER_PLUGIN(scrimmage::Autonomy, scrimmage::autonomy::JoystickAutonomy, JoystickAutonomy_plugin)
 
 namespace scrimmage {
 namespace autonomy {
 
-void JoystickAutonomy::init(std::map<std::string, std::string> &params) {
-    joystick_.init(params, vars_,
-                   std::static_pointer_cast<EntityPlugin>(shared_from_this()));
+void JoystickAutonomy::init(std::map<std::string, std::string>& params) {
+    joystick_.init(params, vars_, std::static_pointer_cast<EntityPlugin>(shared_from_this()));
 }
 
 bool JoystickAutonomy::step_autonomy(double t, double dt) {
     return joystick_.step(t, dt, vars_);
 }
-} // namespace autonomy
-} // namespace scrimmage
+}  // namespace autonomy
+}  // namespace scrimmage
diff --git a/src/plugins/autonomy/MOOSAutonomy/MOOSAutonomy.cpp b/src/plugins/autonomy/MOOSAutonomy/MOOSAutonomy.cpp
index 78be363547..35238ec42d 100644
--- a/src/plugins/autonomy/MOOSAutonomy/MOOSAutonomy.cpp
+++ b/src/plugins/autonomy/MOOSAutonomy/MOOSAutonomy.cpp
@@ -30,12 +30,11 @@
  *
  */
 
-#include <scrimmage/plugin_manager/RegisterPlugin.h>
 #include <scrimmage/entity/Entity.h>
 #include <scrimmage/math/State.h>
-#include <scrimmage/parse/ParseUtils.h>
 #include <scrimmage/parse/MissionParse.h>
-
+#include <scrimmage/parse/ParseUtils.h>
+#include <scrimmage/plugin_manager/RegisterPlugin.h>
 #include <scrimmage/plugins/autonomy/MOOSAutonomy/MOOSAutonomy.h>
 #include <scrimmage/plugins/autonomy/MOOSAutonomy/MOOSNode.h>
 
@@ -59,20 +58,23 @@ MOOSAutonomy::MOOSAutonomy() {
     angles_to_moos_.set_output_zero_axis(ang::HeadingZero::Pos_Y);
 }
 
-void MOOSAutonomy::init(std::map<std::string, std::string> &params) {
+void MOOSAutonomy::init(std::map<std::string, std::string>& params) {
     moos_app_name_ = sc::get<std::string>("moos_app_name", params, "scrimmage");
     moos_app_name_ += std::string("_") + std::to_string(parent_->id().id());
 
     moos_script_ = sc::expand_user(sc::get<std::string>("moos_script", params, "launch.sh"));
-    moos_mission_file_ = sc::expand_user(sc::get<std::string>("moos_mission_file", params, "alpha.moos"));
+    moos_mission_file_ =
+        sc::expand_user(sc::get<std::string>("moos_mission_file", params, "alpha.moos"));
 
-    desired_state_->vel() = Eigen::Vector3d::UnitX()*21;
+    desired_state_->vel() = Eigen::Vector3d::UnitX() * 21;
     desired_state_->quat().set(0, 0, state_->quat().yaw());
-    desired_state_->pos() = Eigen::Vector3d::UnitZ()*state_->pos()(2);
+    desired_state_->pos() = Eigen::Vector3d::UnitZ() * state_->pos()(2);
 
-    desired_alt_idx_ = vars_.declare(VariableIO::Type::desired_altitude, VariableIO::Direction::Out);
+    desired_alt_idx_ =
+        vars_.declare(VariableIO::Type::desired_altitude, VariableIO::Direction::Out);
     desired_speed_idx_ = vars_.declare(VariableIO::Type::desired_speed, VariableIO::Direction::Out);
-    desired_heading_idx_ = vars_.declare(VariableIO::Type::desired_heading, VariableIO::Direction::Out);
+    desired_heading_idx_ =
+        vars_.declare(VariableIO::Type::desired_heading, VariableIO::Direction::Out);
 
     // Kick off moos node thread
     moos_node_.set_time_warp(parent_->mp()->time_warp());
@@ -85,8 +87,7 @@ bool MOOSAutonomy::ready() {
 
 bool MOOSAutonomy::step_autonomy(double t, double dt) {
     // Provide contact locations:
-    for (auto &kv : *contacts_) {
-
+    for (auto& kv : *contacts_) {
         MOOSNode::NodeReportType type;
         if (parent_->id().id() == kv.first) {
             type = MOOSNode::OWNSHIP;
@@ -95,16 +96,13 @@ bool MOOSAutonomy::step_autonomy(double t, double dt) {
         }
 
         // Convert heading to local cartesian
-        sc::StatePtr &s = kv.second.state();
+        sc::StatePtr& s = kv.second.state();
         angles_to_moos_.set_angle(ang::rad2deg(s->quat().yaw()));
         double heading = angles_to_moos_.angle();
 
-        moos_node_.PublishNodeReport(type,
-                                     std::to_string(kv.first), "",
-                                     s->pos()(0), s->pos()(1),
-                                     s->vel().norm(), heading,
-                                     -s->pos()(2), "kayak", "none",
-                                     t, "0");
+        moos_node_.PublishNodeReport(type, std::to_string(kv.first), "", s->pos()(0), s->pos()(1),
+                                     s->vel().norm(), heading, -s->pos()(2), "kayak", "none", t,
+                                     "0");
     }
 
     // Get desired state from moos
@@ -112,8 +110,7 @@ bool MOOSAutonomy::step_autonomy(double t, double dt) {
 
     // Convert heading to local cartesian
     angles_from_moos_.set_angle(ang::rad2deg(s.quat().yaw()));
-    s.quat().set(s.quat().roll(), s.quat().pitch(),
-                 ang::deg2rad(angles_from_moos_.angle()));
+    s.quat().set(s.quat().roll(), s.quat().pitch(), ang::deg2rad(angles_from_moos_.angle()));
 
     desired_state_->vel() = s.vel();
     desired_state_->quat() = s.quat();
@@ -130,5 +127,5 @@ bool MOOSAutonomy::step_autonomy(double t, double dt) {
 void MOOSAutonomy::run_moos_node() {
     moos_node_.Run(moos_app_name_.c_str(), moos_mission_file_.c_str());
 }
-} // namespace autonomy
-} // namespace scrimmage
+}  // namespace autonomy
+}  // namespace scrimmage
diff --git a/src/plugins/autonomy/MOOSAutonomy/MOOSNode.cpp b/src/plugins/autonomy/MOOSAutonomy/MOOSNode.cpp
index f8cca1efb7..d893fc910a 100644
--- a/src/plugins/autonomy/MOOSAutonomy/MOOSNode.cpp
+++ b/src/plugins/autonomy/MOOSAutonomy/MOOSNode.cpp
@@ -30,11 +30,9 @@
  *
  */
 
-
-#include <scrimmage/math/State.h>
-#include <scrimmage/math/Angles.h>
 #include <NodeRecord.h>
-
+#include <scrimmage/math/Angles.h>
+#include <scrimmage/math/State.h>
 #include <scrimmage/plugins/autonomy/MOOSAutonomy/MOOSNode.h>
 
 #include <string>
@@ -44,11 +42,11 @@ namespace sc = scrimmage;
 namespace scrimmage {
 namespace autonomy {
 
-MOOSNode::MOOSNode() : appTick_(1), commsTick_(1), time_warp_(1),
-                       deployed_(false) {
+MOOSNode::MOOSNode() : appTick_(1), commsTick_(1), time_warp_(1), deployed_(false) {
 }
 
-MOOSNode::~MOOSNode() {}
+MOOSNode::~MOOSNode() {
+}
 
 bool MOOSNode::ready() {
     deployed_mutex_.lock();
@@ -68,10 +66,10 @@ void MOOSNode::set_time_warp(double warp) {
     time_warp_ = warp;
 }
 
-bool MOOSNode::OnNewMail(MOOSMSG_LIST &Mail) {
+bool MOOSNode::OnNewMail(MOOSMSG_LIST& Mail) {
     MOOSMSG_LIST::iterator q;
     for (q = Mail.begin(); q != Mail.end(); ++q) {
-        CMOOSMsg &msg = *q;
+        CMOOSMsg& msg = *q;
         std::string key = msg.GetKey();
         double dval = msg.GetDouble();
 
@@ -81,13 +79,13 @@ bool MOOSNode::OnNewMail(MOOSMSG_LIST &Mail) {
             desired_mutex_.unlock();
         } else if (key == "DESIRED_SPEED") {
             desired_mutex_.lock();
-            desired_.vel() = Eigen::Vector3d::UnitX()*dval;
+            desired_.vel() = Eigen::Vector3d::UnitX() * dval;
             desired_mutex_.unlock();
         } else if (key == "DESIRED_DEPTH") {
             desired_mutex_.lock();
-            desired_.pos() = -Eigen::Vector3d::UnitZ()*dval;
+            desired_.pos() = -Eigen::Vector3d::UnitZ() * dval;
             desired_mutex_.unlock();
-        }  else if (key == "IVPHELM_STATE") {
+        } else if (key == "IVPHELM_STATE") {
             if (!deployed_) {
                 Notify("DEPLOY", "true");
                 Notify("RETURN", "false");
@@ -98,7 +96,7 @@ bool MOOSNode::OnNewMail(MOOSMSG_LIST &Mail) {
             }
         }
     }
-    return(true);
+    return (true);
 }
 
 /*
@@ -153,10 +151,8 @@ void MOOSNode::DoRegistrations() {
 }
 
 bool MOOSNode::PublishNodeReport(NodeReportType_t report_type, std::string id,
-                                 std::string sensor_id,
-                                 double nav_x, double nav_y, double speed,
-                                 double heading, double depth,
-                                 std::string type, std::string mode,
+                                 std::string sensor_id, double nav_x, double nav_y, double speed,
+                                 double heading, double depth, std::string type, std::string mode,
                                  double time, std::string frame_number) {
     NodeRecord record;
     record.setName(id);
@@ -171,31 +167,31 @@ bool MOOSNode::PublishNodeReport(NodeReportType_t report_type, std::string id,
 
     std::string moos_var;
     switch (report_type) {
-    case OWNSHIP:
-        moos_var = "NODE_REPORT_LOCAL";
-        Notify("NAV_X", nav_x);
-        Notify("NAV_Y", nav_y);
-        Notify("NAV_HEADING", heading);
-        Notify("NAV_SPEED", speed);
-        Notify("NAV_DEPTH", depth);
-        break;
-
-    case TRUTH_CONTACT:
-        moos_var = "NODE_REPORT";
-        Notify(moos_var, record.getSpec());
-        break;
-
-    case SENSOR_CONTACT:
-        record.setProperty("SENSOR_ID", sensor_id);
-        moos_var = "SENSOR_CONTACT";
-        Notify(moos_var, record.getSpec());
-        break;
-
-    default:
-        break;
+        case OWNSHIP:
+            moos_var = "NODE_REPORT_LOCAL";
+            Notify("NAV_X", nav_x);
+            Notify("NAV_Y", nav_y);
+            Notify("NAV_HEADING", heading);
+            Notify("NAV_SPEED", speed);
+            Notify("NAV_DEPTH", depth);
+            break;
+
+        case TRUTH_CONTACT:
+            moos_var = "NODE_REPORT";
+            Notify(moos_var, record.getSpec());
+            break;
+
+        case SENSOR_CONTACT:
+            record.setProperty("SENSOR_ID", sensor_id);
+            moos_var = "SENSOR_CONTACT";
+            Notify(moos_var, record.getSpec());
+            break;
+
+        default:
+            break;
     }
 
     return true;
 }
-} // namespace autonomy
-} // namespace scrimmage
+}  // namespace autonomy
+}  // namespace scrimmage
diff --git a/src/plugins/autonomy/MotorSchemas/BehaviorBase.cpp b/src/plugins/autonomy/MotorSchemas/BehaviorBase.cpp
index a7b6d1a925..4cfebc85dc 100644
--- a/src/plugins/autonomy/MotorSchemas/BehaviorBase.cpp
+++ b/src/plugins/autonomy/MotorSchemas/BehaviorBase.cpp
@@ -30,40 +30,39 @@
  *
  */
 
-#include <scrimmage/plugins/autonomy/MotorSchemas/BehaviorBase.h>
-
 #include <scrimmage/parse/ParseUtils.h>
+#include <scrimmage/plugins/autonomy/MotorSchemas/BehaviorBase.h>
 
 namespace scrimmage {
 namespace autonomy {
 namespace motor_schemas {
 
-BehaviorBase::BehaviorBase() : desired_vector_(Eigen::Vector3d(0, 0, 0)), gain_(1.0),
-                               max_vector_length_(1.0) {
+BehaviorBase::BehaviorBase()
+    : desired_vector_(Eigen::Vector3d(0, 0, 0)), gain_(1.0), max_vector_length_(1.0) {
 }
 
-Eigen::Vector3d &BehaviorBase::desired_vector() {
+Eigen::Vector3d& BehaviorBase::desired_vector() {
     return desired_vector_;
 }
 
-void BehaviorBase::set_gain(const double &gain) {
+void BehaviorBase::set_gain(const double& gain) {
     gain_ = gain;
 }
 
-const double &BehaviorBase::gain() {
+const double& BehaviorBase::gain() {
     return gain_;
 }
 
-void BehaviorBase::set_max_vector_length(const double &max_vector_length) {
+void BehaviorBase::set_max_vector_length(const double& max_vector_length) {
     max_vector_length_ = max_vector_length;
 }
 
-void BehaviorBase::configure_contacts(std::map<std::string, std::string> &params) {
+void BehaviorBase::configure_contacts(std::map<std::string, std::string>& params) {
     std::vector<std::string> use_contacts;
     if (get_vec("contacts", params, ", ", use_contacts)) {
         use_truth_contacts_ = false;
         use_noisy_contacts_ = false;
-        for (auto &str : use_contacts) {
+        for (auto& str : use_contacts) {
             if (str == "truth") {
                 use_truth_contacts_ = true;
             } else if (str == "noisy") {
@@ -73,13 +72,11 @@ void BehaviorBase::configure_contacts(std::map<std::string, std::string> &params
     }
 
     if (use_noisy_contacts_) {
-        auto cnt_cb = [&] (scrimmage::MessagePtr<ContactMap> &msg) {
-                          noisy_contacts_ = msg->data;
-                      };
+        auto cnt_cb = [&](scrimmage::MessagePtr<ContactMap>& msg) { noisy_contacts_ = msg->data; };
         subscribe<ContactMap>("LocalNetwork", "ContactsWithCovariances", cnt_cb);
     }
 }
 
-} // namespace motor_schemas
-} // namespace autonomy
-} // namespace scrimmage
+}  // namespace motor_schemas
+}  // namespace autonomy
+}  // namespace scrimmage
diff --git a/src/plugins/autonomy/MotorSchemas/MotorSchemas.cpp b/src/plugins/autonomy/MotorSchemas/MotorSchemas.cpp
index 1765c751a3..17e2a05072 100644
--- a/src/plugins/autonomy/MotorSchemas/MotorSchemas.cpp
+++ b/src/plugins/autonomy/MotorSchemas/MotorSchemas.cpp
@@ -30,33 +30,30 @@
  *
  */
 
-#include <Eigen/Geometry>
-
-#include <scrimmage/common/Utilities.h>
+#include <scrimmage/common/ParameterServer.h>
 #include <scrimmage/common/Shape.h>
 #include <scrimmage/common/Time.h>
-#include <scrimmage/common/ParameterServer.h>
+#include <scrimmage/common/Utilities.h>
 #include <scrimmage/entity/Entity.h>
 #include <scrimmage/entity/EntityPluginHelper.h>
-#include <scrimmage/math/State.h>
 #include <scrimmage/math/Angles.h>
+#include <scrimmage/math/State.h>
 #include <scrimmage/parse/ConfigParse.h>
 #include <scrimmage/parse/ParseUtils.h>
-#include <scrimmage/proto/Shape.pb.h>
-#include <scrimmage/proto/ProtoConversions.h>
-#include <scrimmage/plugin_manager/RegisterPlugin.h>
 #include <scrimmage/plugin_manager/PluginManager.h>
-
-#include <scrimmage/plugins/autonomy/MotorSchemas/MotorSchemas.h>
+#include <scrimmage/plugin_manager/RegisterPlugin.h>
 #include <scrimmage/plugins/autonomy/MotorSchemas/BehaviorBase.h>
+#include <scrimmage/plugins/autonomy/MotorSchemas/MotorSchemas.h>
+#include <scrimmage/proto/ProtoConversions.h>
+#include <scrimmage/proto/Shape.pb.h>
 
+#include <Eigen/Geometry>
+#include <boost/algorithm/string/classification.hpp>
+#include <boost/algorithm/string/split.hpp>
+#include <cfloat>
+#include <cmath>
 #include <iostream>
 #include <string>
-#include <cmath>
-#include <cfloat>
-
-#include <boost/algorithm/string/split.hpp>
-#include <boost/algorithm/string/classification.hpp>
 
 using std::cout;
 using std::endl;
@@ -65,14 +62,12 @@ namespace sc = scrimmage;
 namespace sp = scrimmage_proto;
 namespace ms = scrimmage::autonomy::motor_schemas;
 
-REGISTER_PLUGIN(scrimmage::Autonomy,
-                scrimmage::autonomy::MotorSchemas,
-                MotorSchemas_plugin)
+REGISTER_PLUGIN(scrimmage::Autonomy, scrimmage::autonomy::MotorSchemas, MotorSchemas_plugin)
 
 namespace scrimmage {
 namespace autonomy {
 
-void MotorSchemas::init(std::map<std::string, std::string> &params) {
+void MotorSchemas::init(std::map<std::string, std::string>& params) {
     show_shapes_ = sc::get("show_shapes", params, false);
     pub_vel_vec_ = sc::get("pub_vel_vec", params, false);
     max_speed_ = sc::get<double>("max_speed", params, 21);
@@ -80,7 +75,7 @@ void MotorSchemas::init(std::map<std::string, std::string> &params) {
     add_lower_bound_to_vz_ = sc::get("add_lower_bound_to_vz", params, false);
     vz_lower_bound_ = sc::get<double>("vz_lower_bound", params, -1.0);
 
-    auto max_speed_cb = [&] (const double &max_speed) {
+    auto max_speed_cb = [&](const double& max_speed) {
         cout << "MotorSchemas Max speed set: " << max_speed << endl;
     };
     register_param<double>("max_speed", max_speed_, max_speed_cb);
@@ -88,7 +83,7 @@ void MotorSchemas::init(std::map<std::string, std::string> &params) {
     // Subscribe to state information
     std::string state_topic_name = sc::get<std::string>("state_topic_name", params, "State");
     std::string network_name = sc::get<std::string>("network_name", params, "LocalNetwork");
-    auto state_callback = [&] (scrimmage::MessagePtr<std::string> msg) {
+    auto state_callback = [&](scrimmage::MessagePtr<std::string> msg) {
         current_state_ = msg->data;
 
         // Get the currently running behaviors
@@ -103,17 +98,17 @@ void MotorSchemas::init(std::map<std::string, std::string> &params) {
 
     // Parse the autonomy vectors
     std::list<PluginOverrides> plugin_overrides_list;
-    if (sc::parse_plugin_vector("behaviors", params, plugin_overrides_list) == static_cast<unsigned int>(0)) {
+    if (sc::parse_plugin_vector("behaviors", params, plugin_overrides_list)
+        == static_cast<unsigned int>(0)) {
         cout << "MotorSchemas: Failed to parse any behaviors." << endl;
     }
 
     // Create the plugin for each autonomy/behavior
-    for (auto &plugin_override : plugin_overrides_list) {
+    for (auto& plugin_override : plugin_overrides_list) {
         auto behavior = make_autonomy<ms::BehaviorBase>(
-            plugin_override.name, parent_->plugin_manager(),
-            plugin_override.overrides, parent_, state_, id_to_team_map_,
-            id_to_ent_map_, proj_, contacts_, parent_->file_search(), rtree_,
-            parent_->pubsub(), time_, parent_->param_server());
+            plugin_override.name, parent_->plugin_manager(), plugin_override.overrides, parent_,
+            state_, id_to_team_map_, id_to_ent_map_, proj_, contacts_, parent_->file_search(),
+            rtree_, parent_->pubsub(), time_, parent_->param_server());
 
         if (behavior) {
             // Extract the gain for this plugin and apply it
@@ -134,16 +129,18 @@ void MotorSchemas::init(std::map<std::string, std::string> &params) {
 
     // Add the default behaviors to each declared state
     for (motor_schemas::BehaviorBasePtr behavior : default_behaviors_) {
-        for (auto &kv : behaviors_) {
+        for (auto& kv : behaviors_) {
             kv.second.push_back(behavior);
         }
     }
 
     current_behaviors_ = default_behaviors_;
 
-    desired_alt_idx_ = vars_.declare(VariableIO::Type::desired_altitude, VariableIO::Direction::Out);
+    desired_alt_idx_ =
+        vars_.declare(VariableIO::Type::desired_altitude, VariableIO::Direction::Out);
     desired_speed_idx_ = vars_.declare(VariableIO::Type::desired_speed, VariableIO::Direction::Out);
-    desired_heading_idx_ = vars_.declare(VariableIO::Type::desired_heading, VariableIO::Direction::Out);
+    desired_heading_idx_ =
+        vars_.declare(VariableIO::Type::desired_heading, VariableIO::Direction::Out);
 
     output_vel_x_idx_ = vars_.declare(VariableIO::Type::velocity_x, VariableIO::Direction::Out);
     output_vel_y_idx_ = vars_.declare(VariableIO::Type::velocity_y, VariableIO::Direction::Out);
@@ -155,14 +152,14 @@ bool MotorSchemas::step_autonomy(double t, double dt) {
     double vec_w_gain_sum = 0;
     Eigen::Vector3d vec_w_gain(0, 0, 0);
 
-    for (motor_schemas::BehaviorBasePtr &behavior : current_behaviors_) {
+    for (motor_schemas::BehaviorBasePtr& behavior : current_behaviors_) {
         behavior->shapes().clear();
 
         // cout << "Behavior: " << behavior->name() << endl;
 
         // Execute callbacks for received messages before calling
         // step_autonomy
-        for (SubscriberBasePtr &sub : behavior->subs()) {
+        for (SubscriberBasePtr& sub : behavior->subs()) {
             for (auto msg : sub->pop_msgs<sc::MessageBase>()) {
                 sub->accept(msg);
             }
@@ -178,8 +175,7 @@ bool MotorSchemas::step_autonomy(double t, double dt) {
         }
 
         if (desired_vector.hasNaN()) {
-            cout << "Behavior error: " << behavior->name()
-                 << ", desired vector has NaN" << endl;
+            cout << "Behavior error: " << behavior->name() << ", desired vector has NaN" << endl;
             continue;
         }
 
@@ -197,10 +193,8 @@ bool MotorSchemas::step_autonomy(double t, double dt) {
         }
 
         if (show_shapes_) {
-             std::for_each(behavior->shapes().begin(),
-                           behavior->shapes().end(), [&](auto &s) {
-                               this->draw_shape(s);
-                           });
+            std::for_each(behavior->shapes().begin(), behavior->shapes().end(),
+                          [&](auto& s) { this->draw_shape(s); });
         }
         behavior->shapes().clear();
     }
@@ -238,9 +232,8 @@ bool MotorSchemas::step_autonomy(double t, double dt) {
     if (show_shapes_) {
         // Draw resultant vector:
         if (line_shape_ == nullptr) {
-            line_shape_ = sc::shape::make_line(
-                state_->pos(), vel_result + state_->pos(),
-                Eigen::Vector3d(255, 255, 0), 0.75);
+            line_shape_ = sc::shape::make_line(state_->pos(), vel_result + state_->pos(),
+                                               Eigen::Vector3d(255, 255, 0), 0.75);
         }
         sc::set(line_shape_->mutable_line()->mutable_start(), state_->pos());
         sc::set(line_shape_->mutable_line()->mutable_end(), vel_result + state_->pos());
@@ -248,5 +241,5 @@ bool MotorSchemas::step_autonomy(double t, double dt) {
     }
     return true;
 }
-} // namespace autonomy
-} // namespace scrimmage
+}  // namespace autonomy
+}  // namespace scrimmage
diff --git a/src/plugins/autonomy/MoveToGoalMS/MoveToGoalMS.cpp b/src/plugins/autonomy/MoveToGoalMS/MoveToGoalMS.cpp
index aea1517671..3ccb016293 100644
--- a/src/plugins/autonomy/MoveToGoalMS/MoveToGoalMS.cpp
+++ b/src/plugins/autonomy/MoveToGoalMS/MoveToGoalMS.cpp
@@ -30,31 +30,28 @@
  *
  */
 
-#include <scrimmage/plugins/autonomy/MoveToGoalMS/MoveToGoalMS.h>
-
-#include <scrimmage/plugin_manager/RegisterPlugin.h>
 #include <scrimmage/entity/Entity.h>
 #include <scrimmage/math/State.h>
 #include <scrimmage/parse/ParseUtils.h>
+#include <scrimmage/plugin_manager/RegisterPlugin.h>
+#include <scrimmage/plugins/autonomy/MoveToGoalMS/MoveToGoalMS.h>
+#include <scrimmage/proto/ProtoConversions.h>
+#include <scrimmage/proto/Shape.pb.h>
 #include <scrimmage/pubsub/Message.h>
 #include <scrimmage/pubsub/Subscriber.h>
-#include <scrimmage/proto/Shape.pb.h>
-#include <scrimmage/proto/ProtoConversions.h>
 
+#include <GeographicLib/LocalCartesian.hpp>
+#include <cmath>
 #include <iostream>
 #include <limits>
-#include <cmath>
 #include <list>
 
-#include <GeographicLib/LocalCartesian.hpp>
-
 using std::cout;
 using std::endl;
 
 namespace sc = scrimmage;
 
-REGISTER_PLUGIN(scrimmage::Autonomy,
-                scrimmage::autonomy::motor_schemas::MoveToGoalMS,
+REGISTER_PLUGIN(scrimmage::Autonomy, scrimmage::autonomy::motor_schemas::MoveToGoalMS,
                 MoveToGoalMS_plugin)
 
 namespace scrimmage {
@@ -64,9 +61,9 @@ namespace motor_schemas {
 MoveToGoalMS::MoveToGoalMS() {
 }
 
-void MoveToGoalMS::init(std::map<std::string, std::string> &params) {
+void MoveToGoalMS::init(std::map<std::string, std::string>& params) {
     if (sc::get("use_initial_heading", params, false)) {
-        Eigen::Vector3d rel_pos = Eigen::Vector3d::UnitX()*1e6;
+        Eigen::Vector3d rel_pos = Eigen::Vector3d::UnitX() * 1e6;
         Eigen::Vector3d unit_vector = rel_pos.normalized();
         unit_vector = state_->quat().rotate(unit_vector);
         wp_local_ = state_->pos() + unit_vector * rel_pos.norm();
@@ -80,16 +77,14 @@ void MoveToGoalMS::init(std::map<std::string, std::string> &params) {
     }
 
     // Initialize PID controller class
-    speed_pid_.set_parameters(sc::get("p_gain", params, 0.5),
-                              sc::get("i_gain", params, 0.0),
+    speed_pid_.set_parameters(sc::get("p_gain", params, 0.5), sc::get("i_gain", params, 0.0),
                               sc::get("d_gain", params, 0.0));
     speed_pid_.set_integral_band(sc::get("integral_band", params, 0.0));
     speed_pid_.set_setpoint(0.0);
 
     // Convert XYZ goal to lat/lon/alt
     double lat, lon, alt;
-    parent_->projection()->Reverse(wp_local_(0), wp_local_(1),
-                                   wp_local_(2), lat, lon, alt);
+    parent_->projection()->Reverse(wp_local_(0), wp_local_(1), wp_local_(2), lat, lon, alt);
 
     wp_ = Waypoint(lat, lon, alt);
     wp_.set_time(0);
@@ -97,12 +92,10 @@ void MoveToGoalMS::init(std::map<std::string, std::string> &params) {
     wp_.set_position_tolerance(1);
     wp_.set_quat_tolerance(1);
 
-    auto wp_cb = [&] (scrimmage::MessagePtr<Waypoint> msg) {
+    auto wp_cb = [&](scrimmage::MessagePtr<Waypoint> msg) {
         wp_ = msg->data;
-        parent_->projection()->Forward(wp_.latitude(),
-                                       wp_.longitude(),
-                                       wp_.altitude(), wp_local_(0),
-                                       wp_local_(1), wp_local_(2));
+        parent_->projection()->Forward(wp_.latitude(), wp_.longitude(), wp_.altitude(),
+                                       wp_local_(0), wp_local_(1), wp_local_(2));
     };
     subscribe<Waypoint>("LocalNetwork", "Waypoint", wp_cb);
 }
@@ -113,6 +106,6 @@ bool MoveToGoalMS::step_autonomy(double t, double dt) {
     desired_vector_ = (wp_local_ - state_->pos()).normalized() * speed_factor;
     return true;
 }
-} // namespace motor_schemas
-} // namespace autonomy
-} // namespace scrimmage
+}  // namespace motor_schemas
+}  // namespace autonomy
+}  // namespace scrimmage
diff --git a/src/plugins/autonomy/Predator/Predator.cpp b/src/plugins/autonomy/Predator/Predator.cpp
index 4ad6b7757c..8fecbc2c02 100644
--- a/src/plugins/autonomy/Predator/Predator.cpp
+++ b/src/plugins/autonomy/Predator/Predator.cpp
@@ -31,14 +31,14 @@
  */
 
 #include <scrimmage/entity/Entity.h>
-#include <scrimmage/math/State.h>
 #include <scrimmage/math/Angles.h>
+#include <scrimmage/math/State.h>
 #include <scrimmage/msgs/Capture.pb.h>
 #include <scrimmage/parse/ParseUtils.h>
 #include <scrimmage/plugin_manager/RegisterPlugin.h>
+#include <scrimmage/plugins/autonomy/Predator/Predator.h>
 #include <scrimmage/pubsub/Message.h>
 #include <scrimmage/pubsub/Publisher.h>
-#include <scrimmage/plugins/autonomy/Predator/Predator.h>
 
 #include <limits>
 
@@ -49,7 +49,7 @@ REGISTER_PLUGIN(scrimmage::Autonomy, scrimmage::autonomy::Predator, Predator_plu
 namespace scrimmage {
 namespace autonomy {
 
-void Predator::init(std::map<std::string, std::string> &params) {
+void Predator::init(std::map<std::string, std::string>& params) {
     max_speed_ = get<double>("max_speed", params, 21);
     capture_range_ = get<double>("capture_range", params, 5);
     prey_team_id_ = get<int>("prey_team_id", params, 1);
@@ -64,7 +64,8 @@ void Predator::init(std::map<std::string, std::string> &params) {
     turn_rate_idx_ = vars_.declare(VariableIO::Type::turn_rate, VariableIO::Direction::Out);
     pitch_rate_idx_ = vars_.declare(VariableIO::Type::pitch_rate, VariableIO::Direction::Out);
 
-    desired_heading_idx_ = vars_.declare(VariableIO::Type::desired_heading, VariableIO::Direction::Out);
+    desired_heading_idx_ =
+        vars_.declare(VariableIO::Type::desired_heading, VariableIO::Direction::Out);
     desired_speed_idx_ = vars_.declare(VariableIO::Type::desired_speed, VariableIO::Direction::Out);
 }
 
@@ -132,5 +133,5 @@ bool Predator::step_autonomy(double t, double dt) {
 
     return true;
 }
-} // namespace autonomy
-} // namespace scrimmage
+}  // namespace autonomy
+}  // namespace scrimmage
diff --git a/src/plugins/autonomy/PyAutonomy/PyAutonomy.cpp b/src/plugins/autonomy/PyAutonomy/PyAutonomy.cpp
index 08066dd950..2d2d235689 100644
--- a/src/plugins/autonomy/PyAutonomy/PyAutonomy.cpp
+++ b/src/plugins/autonomy/PyAutonomy/PyAutonomy.cpp
@@ -30,30 +30,27 @@
  *
  */
 
-#include <scrimmage/plugins/autonomy/PyAutonomy/PyAutonomy.h>
-
+#include <pybind11/eigen.h>
+#include <pybind11/embed.h>
+#include <pybind11/pybind11.h>
 #include <scrimmage/entity/Entity.h>
+#include <scrimmage/math/Quaternion.h>
 #include <scrimmage/math/State.h>
 #include <scrimmage/plugin_manager/RegisterPlugin.h>
-#include <scrimmage/math/Quaternion.h>
+#include <scrimmage/plugins/autonomy/PyAutonomy/PyAutonomy.h>
+#include <scrimmage/proto/Shape.pb.h>
 #include <scrimmage/pubsub/Message.h>
-#include <scrimmage/pubsub/Subscriber.h>
 #include <scrimmage/pubsub/Publisher.h>
-#include <scrimmage/proto/Shape.pb.h>
+#include <scrimmage/pubsub/Subscriber.h>
 
-#include <pybind11/pybind11.h>
-#include <pybind11/embed.h>
-#include <pybind11/eigen.h>
 #include <cstddef>
-#include <stdexcept>
 #include <iostream>
+#include <stdexcept>
 
 using std::cout;
 using std::endl;
 
-REGISTER_PLUGIN(scrimmage::Autonomy,
-                scrimmage::autonomy::PyAutonomy,
-                PyAutonomy_plugin)
+REGISTER_PLUGIN(scrimmage::Autonomy, scrimmage::autonomy::PyAutonomy, PyAutonomy_plugin)
 
 namespace py = pybind11;
 namespace sc = scrimmage;
@@ -68,22 +65,22 @@ PyAutonomy::PyAutonomy() {
     need_reset_ = true;
 }
 
-void PyAutonomy::init(std::map<std::string, std::string> &params) {
+void PyAutonomy::init(std::map<std::string, std::string>& params) {
     py_obj_ = get_py_obj(params);
     py_obj_.attr("id") = py::cast(parent_->id());
     init_py_obj(params);
 }
 
-py::object PyAutonomy::get_py_obj(std::map<std::string, std::string> &params) {
+py::object PyAutonomy::get_py_obj(std::map<std::string, std::string>& params) {
     py::module module = py::module::import(params["module"].c_str());
     py::object py_obj_class = module.attr(params["class"].c_str());
     py_obj_ = py_obj_class();
     return py_obj_;
 }
 
-void PyAutonomy::init_py_obj(std::map<std::string, std::string> &params) {
+void PyAutonomy::init_py_obj(std::map<std::string, std::string>& params) {
     py::dict py_params;
-    for (auto &kv : params) {
+    for (auto& kv : params) {
         if (kv.first != "module" && kv.first != "class" && kv.first != "library") {
             py_params[kv.first.c_str()] = py::str(kv.second);
         }
@@ -116,7 +113,7 @@ void PyAutonomy::cache_python_vars() {
     }
 }
 
-py::object PyAutonomy::state2py(sc::StatePtr &state) {
+py::object PyAutonomy::state2py(sc::StatePtr& state) {
     cache_python_vars();
 
     sc::Quaternion quat = state->quat();
@@ -247,7 +244,7 @@ bool PyAutonomy::step_autonomy(double t, double dt) {
     *desired_state_ = state;
 
     py::list py_shapes = py_obj_.attr("shapes").cast<py::list>();
-    std::list< std::shared_ptr<scrimmage_proto::Shape> > cpp_shapes;
+    std::list<std::shared_ptr<scrimmage_proto::Shape> > cpp_shapes;
     std::transform(py_shapes.begin(), py_shapes.end(), std::back_inserter(cpp_shapes), py2shape);
     // shapes_ = cpp_shapes; // SHAPES TODO
 
@@ -255,5 +252,5 @@ bool PyAutonomy::step_autonomy(double t, double dt) {
 
     return out;
 }
-} // namespace autonomy
-} // namespace scrimmage
+}  // namespace autonomy
+}  // namespace scrimmage
diff --git a/src/plugins/autonomy/RLConsensus/RLConsensus.cpp b/src/plugins/autonomy/RLConsensus/RLConsensus.cpp
index 1eb1f3c7d0..274811aff5 100644
--- a/src/plugins/autonomy/RLConsensus/RLConsensus.cpp
+++ b/src/plugins/autonomy/RLConsensus/RLConsensus.cpp
@@ -36,13 +36,11 @@
 #include <scrimmage/parse/MissionParse.h>
 #include <scrimmage/parse/ParseUtils.h>
 #include <scrimmage/plugin_manager/RegisterPlugin.h>
-
 #include <scrimmage/plugins/autonomy/RLConsensus/RLConsensus.h>
 
-#include <iostream>
-
-#include <boost/range/algorithm/count_if.hpp>
 #include <boost/range/adaptor/transformed.hpp>
+#include <boost/range/algorithm/count_if.hpp>
+#include <iostream>
 
 namespace sp = scrimmage_proto;
 namespace br = boost::range;
@@ -61,8 +59,8 @@ void RLConsensus::set_environment() {
 std::tuple<bool, double, pybind11::dict> RLConsensus::calc_reward() {
     const bool done = false;
     const double x = state_->pos()(0);
-    auto dist = [&](auto &kv) {return std::abs(kv.second.state()->pos()(0) - x);};
-    auto close = [&](double d) {return std::abs(d) < radius_;};
+    auto dist = [&](auto& kv) { return std::abs(kv.second.state()->pos()(0) - x); };
+    auto close = [&](double d) { return std::abs(d) < radius_; };
 
     const int num_veh = contacts_->size();
     const int num_close = br::count_if(*contacts_ | ba::transformed(dist), close) - 1;
@@ -71,5 +69,5 @@ std::tuple<bool, double, pybind11::dict> RLConsensus::calc_reward() {
     return std::make_tuple(done, reward, pybind11::dict());
 }
 
-} // namespace autonomy
-} // namespace scrimmage
+}  // namespace autonomy
+}  // namespace scrimmage
diff --git a/src/plugins/autonomy/RLSimple/RLSimple.cpp b/src/plugins/autonomy/RLSimple/RLSimple.cpp
index 9f2172ce04..7daa1e6d72 100644
--- a/src/plugins/autonomy/RLSimple/RLSimple.cpp
+++ b/src/plugins/autonomy/RLSimple/RLSimple.cpp
@@ -34,7 +34,6 @@
 #include <scrimmage/math/State.h>
 #include <scrimmage/parse/ParseUtils.h>
 #include <scrimmage/plugin_manager/RegisterPlugin.h>
-
 #include <scrimmage/plugins/autonomy/RLSimple/RLSimple.h>
 
 REGISTER_PLUGIN(scrimmage::Autonomy, scrimmage::autonomy::RLSimple, RLSimple_plugin)
@@ -42,7 +41,7 @@ REGISTER_PLUGIN(scrimmage::Autonomy, scrimmage::autonomy::RLSimple, RLSimple_plu
 namespace scrimmage {
 namespace autonomy {
 
-void RLSimple::init_helper(std::map<std::string, std::string> &params) {
+void RLSimple::init_helper(std::map<std::string, std::string>& params) {
     x_discrete_ = str2bool(params.at("x_discrete"));
     y_discrete_ = str2bool(params.at("y_discrete"));
     ctrl_y_ = str2bool(params.at("ctrl_y"));
@@ -71,7 +70,7 @@ void RLSimple::set_environment() {
 
     auto add_output = [&](bool is_discrete) {
         if (is_discrete) {
-            action_space.discrete_count.push_back(2); // forward/backward
+            action_space.discrete_count.push_back(2);  // forward/backward
         } else {
             action_space.continuous_extrema.push_back({-max_speed_, max_speed_});
         }
@@ -90,7 +89,7 @@ std::tuple<bool, double, pybind11::dict> RLSimple::calc_reward() {
     double reward = within_radius ? 1 : 0;
 
     pybind11::dict info;
-    info["x_within_radius"] = within_radius; // added for test harness
+    info["x_within_radius"] = within_radius;  // added for test harness
     return std::make_tuple(done, reward, info);
 }
 
@@ -115,5 +114,5 @@ bool RLSimple::step_helper() {
     return true;
 }
 
-} // namespace autonomy
-} // namespace scrimmage
+}  // namespace autonomy
+}  // namespace scrimmage
diff --git a/src/plugins/autonomy/ROSAirSim/ROSAirSim.cpp b/src/plugins/autonomy/ROSAirSim/ROSAirSim.cpp
index 821cdffd42..836960b9f4 100644
--- a/src/plugins/autonomy/ROSAirSim/ROSAirSim.cpp
+++ b/src/plugins/autonomy/ROSAirSim/ROSAirSim.cpp
@@ -28,23 +28,20 @@
  * Receives AirSim data as SCRIMMAGE messages and publishes them as ROS messages.
  *
  */
-#include <scrimmage/plugins/autonomy/ROSAirSim/ROSAirSim.h>
-#include <scrimmage/plugins/sensor/AirSimSensor/AirSimSensor.h>
-#include <scrimmage/plugin_manager/RegisterPlugin.h>
+#include <rosgraph_msgs/Clock.h>
 #include <scrimmage/entity/Entity.h>
 #include <scrimmage/math/State.h>
 #include <scrimmage/parse/ParseUtils.h>
+#include <scrimmage/plugin_manager/RegisterPlugin.h>
+#include <scrimmage/plugins/autonomy/ROSAirSim/ROSAirSim.h>
+#include <scrimmage/plugins/sensor/AirSimSensor/AirSimSensor.h>
 
-#include <rosgraph_msgs/Clock.h>
-
+#include <boost/algorithm/string.hpp>
 #include <iostream>
 #include <limits>
-
 #include <opencv2/core/core.hpp>
 #include <opencv2/highgui/highgui.hpp>
 
-#include <boost/algorithm/string.hpp>
-
 using std::cout;
 using std::endl;
 
@@ -55,9 +52,11 @@ REGISTER_PLUGIN(scrimmage::Autonomy, scrimmage::autonomy::ROSAirSim, ROSAirSim_p
 namespace scrimmage {
 namespace autonomy {
 
-ROSAirSim::ROSAirSim() {}
+ROSAirSim::ROSAirSim() {
+}
 
-Eigen::Isometry3f ROSAirSim::get_vehicle_world_pose_from_NED_to_ENU(Eigen::Isometry3f vehicle_pose_world_NED) {
+Eigen::Isometry3f ROSAirSim::get_vehicle_world_pose_from_NED_to_ENU(
+    Eigen::Isometry3f vehicle_pose_world_NED) {
     // Get Vehicle Pose from NED to ENU
     Eigen::Matrix<float, 3, 1> vehicle_position_ENU;
     vehicle_position_ENU.x() = vehicle_pose_world_NED.translation().y();
@@ -68,11 +67,13 @@ Eigen::Isometry3f ROSAirSim::get_vehicle_world_pose_from_NED_to_ENU(Eigen::Isome
     Eigen::Quaternionf NED_quat_vehicle(vehicle_pose_world_NED.rotation());
 
     // Rotate, order matters
-    double xTo = -180 * (M_PI / 180); // -PI rotation about X
-    double zTo = 90 * (M_PI / 180); // PI/2 rotation about Z (Up)
+    double xTo = -180 * (M_PI / 180);  // -PI rotation about X
+    double zTo = 90 * (M_PI / 180);    // PI/2 rotation about Z (Up)
     Eigen::Quaternion<float> vehicle_orientation_ENU;
-    vehicle_orientation_ENU = Eigen::AngleAxis<float>(xTo, Eigen::Vector3f::UnitX()) * NED_quat_vehicle; // -PI rotation about X
-    vehicle_orientation_ENU = Eigen::AngleAxis<float>(zTo, Eigen::Vector3f::UnitZ()) * vehicle_orientation_ENU; // PI/2 rotation about Z (Up)
+    vehicle_orientation_ENU = Eigen::AngleAxis<float>(xTo, Eigen::Vector3f::UnitX())
+                              * NED_quat_vehicle;  // -PI rotation about X
+    vehicle_orientation_ENU = Eigen::AngleAxis<float>(zTo, Eigen::Vector3f::UnitZ())
+                              * vehicle_orientation_ENU;  // PI/2 rotation about Z (Up)
     // Bring vehicle pose in relation to ENU, World into Eigen
     Eigen::Translation3f translation_trans_vehicle(vehicle_position_ENU);
     Eigen::Quaternionf rotation_quat_vehicle(vehicle_orientation_ENU);
@@ -80,10 +81,11 @@ Eigen::Isometry3f ROSAirSim::get_vehicle_world_pose_from_NED_to_ENU(Eigen::Isome
     return tf_world_vehicle_ENU;
 }
 
-Eigen::Isometry3f ROSAirSim::get_sensor_pose_from_worldNED_to_vehicleENU(Eigen::Isometry3f vehicle_pose_world_NED,
-                                                                         Eigen::Isometry3f sensor_pose_world_NED) {
+Eigen::Isometry3f ROSAirSim::get_sensor_pose_from_worldNED_to_vehicleENU(
+    Eigen::Isometry3f vehicle_pose_world_NED, Eigen::Isometry3f sensor_pose_world_NED) {
     // Get Vehicle Pose from NED to ENU
-    Eigen::Isometry3f tf_world_vehicle_ENU = get_vehicle_world_pose_from_NED_to_ENU(vehicle_pose_world_NED);
+    Eigen::Isometry3f tf_world_vehicle_ENU =
+        get_vehicle_world_pose_from_NED_to_ENU(vehicle_pose_world_NED);
 
     // AirSim gives pose of camera in relation to the world frame
     // Pose in response is in NED, but scrimmage is in ENU so convert
@@ -96,11 +98,13 @@ Eigen::Isometry3f ROSAirSim::get_sensor_pose_from_worldNED_to_vehicleENU(Eigen::
     // Eigen::Quaternion<float> NED_quat = sensor_pose_world_NED.rotation();
     Eigen::Quaternionf NED_quat(sensor_pose_world_NED.rotation());
     // Rotate, order matters
-    double xTo = -180 * (M_PI / 180); // -PI rotation about X
-    double zTo = 90 * (M_PI / 180); // PI/2 rotation about Z (Up)
+    double xTo = -180 * (M_PI / 180);  // -PI rotation about X
+    double zTo = 90 * (M_PI / 180);    // PI/2 rotation about Z (Up)
     Eigen::Quaternion<float> sensor_orientation_world_ENU;
-    sensor_orientation_world_ENU = Eigen::AngleAxis<float>(xTo, Eigen::Vector3f::UnitX()) * NED_quat; // -PI rotation about X
-    sensor_orientation_world_ENU = Eigen::AngleAxis<float>(zTo, Eigen::Vector3f::UnitZ()) * sensor_orientation_world_ENU; // PI/2 rotation about Z (Up)
+    sensor_orientation_world_ENU =
+        Eigen::AngleAxis<float>(xTo, Eigen::Vector3f::UnitX()) * NED_quat;  // -PI rotation about X
+    sensor_orientation_world_ENU = Eigen::AngleAxis<float>(zTo, Eigen::Vector3f::UnitZ())
+                                   * sensor_orientation_world_ENU;  // PI/2 rotation about Z (Up)
     // Place pose of camera in ENU, world frame into Eigen
     Eigen::Translation3f translation_trans_sensor(sensor_position_world_ENU);
     Eigen::Quaternionf rotation_quat_sensor(sensor_orientation_world_ENU);
@@ -111,7 +115,7 @@ Eigen::Isometry3f ROSAirSim::get_sensor_pose_from_worldNED_to_vehicleENU(Eigen::
     return tf_vehicle_sensor_ENU;
 }
 
-void ROSAirSim::init(std::map<std::string, std::string> &params) {
+void ROSAirSim::init(std::map<std::string, std::string>& params) {
     vehicle_name_ = sc::get<std::string>("vehicle_name", params, "robot1");
     show_camera_images_ = scrimmage::get<bool>("show_camera_images", params, "false");
     pub_image_data_ = sc::get<bool>("pub_image_data", params, "true");
@@ -167,7 +171,7 @@ void ROSAirSim::init(std::map<std::string, std::string> &params) {
     tf_msg_vec_.clear();
 
     // Create World Transform to Robot
-    sc::StatePtr &state = parent_->state_truth();
+    sc::StatePtr& state = parent_->state_truth();
     world_trans_.header.frame_id = "world";
     if (ros_cartographer_) {
         world_trans_.child_frame_id = ros_namespace_;
@@ -202,9 +206,9 @@ void ROSAirSim::init(std::map<std::string, std::string> &params) {
     tf_msg_vec_.clear();
 
     // airsim image callback
-    auto airsim_image_cb = [&](auto &msg) {
+    auto airsim_image_cb = [&](auto& msg) {
         image_data_ = msg->data;
-        if (image_data_.empty()) { // return if empty message
+        if (image_data_.empty()) {  // return if empty message
             return;
         }
 
@@ -216,18 +220,19 @@ void ROSAirSim::init(std::map<std::string, std::string> &params) {
         if (!img_topic_published) {
             // for each image in the message
             for (sc::sensor::AirSimImageType a : image_data_) {
-
                 // Create the topic name -- ROS needs lowercase names
                 std::string camera_name = boost::algorithm::to_lower_copy(a.camera_config.cam_name);
-                std::string image_type_name = boost::algorithm::to_lower_copy(a.camera_config.img_type_name);
+                std::string image_type_name =
+                    boost::algorithm::to_lower_copy(a.camera_config.img_type_name);
                 std::string cam_topic_name = ros_namespace_ + "/camera/" + camera_name;
                 std::string topic_name_pub = cam_topic_name + "/" + image_type_name;
                 std::string info_topic_name_pub = topic_name_pub + "/info";
-                topic_name_pub += + "/raw";
+                topic_name_pub += +"/raw";
                 // std::string topic_name = "/" + topic_name_pub;
 
                 if (ros_python_) {
-                    img_publishers_.push_back(nh_->advertise<sensor_msgs::Image>(topic_name_pub, 1));
+                    img_publishers_.push_back(
+                        nh_->advertise<sensor_msgs::Image>(topic_name_pub, 1));
                 } else {
                     trans_img_publishers_.push_back(it_->advertise(topic_name_pub, 1));
                 }
@@ -246,17 +251,19 @@ void ROSAirSim::init(std::map<std::string, std::string> &params) {
                 info_msg.distortion_model = "plumb_bob";
                 info_msg.K = {fx, 0.f, cx, 0.f, fy, cy, 0.f, 0.f, 1.f};
                 info_msg.R = {1.f, 0.f, 0.f, 0.f, 1.f, 0.f, 0.f, 0.f, 1.f};
-                info_msg.P = {fx, 0.f, cx, 0.f, 0.f, fy, cy,  0.f,  0.f, 0.f, 1.f, 0.f};
-                cam_info_publishers_.push_back(nh_->advertise<sensor_msgs::CameraInfo>(info_topic_name_pub, 1));
+                info_msg.P = {fx, 0.f, cx, 0.f, 0.f, fy, cy, 0.f, 0.f, 0.f, 1.f, 0.f};
+                cam_info_publishers_.push_back(
+                    nh_->advertise<sensor_msgs::CameraInfo>(info_topic_name_pub, 1));
                 // publish using last publisher created
                 cam_info_publishers_.back().publish(info_msg);
 
                 // Publish current message so we don't miss any images
                 sensor_msgs::ImagePtr img_msg;
-                std_msgs::Header header; // empty header
-                header.stamp = ros::Time::now(); // time
+                std_msgs::Header header;          // empty header
+                header.stamp = ros::Time::now();  // time
                 header.frame_id = topic_name_pub + "_link";
-                // Depth Images (Depth Perspective and Depth Planner) come in as 1 channel float arrays
+                // Depth Images (Depth Perspective and Depth Planner) come in as 1 channel float
+                // arrays
                 if (a.camera_config.pixels_as_float) {
                     img_msg = cv_bridge::CvImage(header, "", a.img).toImageMsg();
                 } else {
@@ -272,7 +279,9 @@ void ROSAirSim::init(std::map<std::string, std::string> &params) {
                 //// publish a transform for each unique camera_name
                 // Note down camera names since each camera will need its own image transform
                 // old_cam_name=true if camera_name already exists
-                bool old_cam_name = std::any_of(camera_names_.begin(), camera_names_.end(), [camera_name](std::string str){return str == camera_name;});
+                bool old_cam_name =
+                    std::any_of(camera_names_.begin(), camera_names_.end(),
+                                [camera_name](std::string str) { return str == camera_name; });
                 // If false, this is a new camera name, save and publish transform
                 if (!old_cam_name) {
                     // cout << "New camera found: " << camera_name << endl;
@@ -281,10 +290,12 @@ void ROSAirSim::init(std::map<std::string, std::string> &params) {
                     tf_msg_vec_.clear();
 
                     // Get Vehicle Pose from NED to ENU
-                    Eigen::Isometry3f tf_world_vehicle_ENU = get_vehicle_world_pose_from_NED_to_ENU(a.vehicle_pose_world_NED);
+                    Eigen::Isometry3f tf_world_vehicle_ENU =
+                        get_vehicle_world_pose_from_NED_to_ENU(a.vehicle_pose_world_NED);
                     // Get pose of lidar in ENU, vehicle frame using Eigen
-                    Eigen::Isometry3f tf_vehicle_camera_ENU = get_sensor_pose_from_worldNED_to_vehicleENU(a.vehicle_pose_world_NED,
-                                                                                                          a.camera_pose_world_NED);
+                    Eigen::Isometry3f tf_vehicle_camera_ENU =
+                        get_sensor_pose_from_worldNED_to_vehicleENU(a.vehicle_pose_world_NED,
+                                                                    a.camera_pose_world_NED);
 
                     //////////////////////////////////////////////////////////////////////
                     // Update and Publish Transforms
@@ -334,7 +345,8 @@ void ROSAirSim::init(std::map<std::string, std::string> &params) {
                     image_trans_.transform.translation.y = tf_vehicle_camera_ENU.translation().y();
                     image_trans_.transform.translation.z = tf_vehicle_camera_ENU.translation().z();
                     // Orientation
-                    Eigen::Quaternionf tf_vehicle_camera_ENU_rotation(tf_vehicle_camera_ENU.rotation());
+                    Eigen::Quaternionf tf_vehicle_camera_ENU_rotation(
+                        tf_vehicle_camera_ENU.rotation());
                     image_trans_.transform.rotation.w = tf_vehicle_camera_ENU_rotation.w();
                     image_trans_.transform.rotation.x = tf_vehicle_camera_ENU_rotation.x();
                     image_trans_.transform.rotation.y = tf_vehicle_camera_ENU_rotation.y();
@@ -343,20 +355,20 @@ void ROSAirSim::init(std::map<std::string, std::string> &params) {
 
                     laser_broadcaster_->sendTransform(tf_msg_vec_);
                     tf_msg_vec_.clear();
-                } // end if new camera name to publish pose for
-            } // end for loop a : images in msg
+                }  // end if new camera name to publish pose for
+            }  // end for loop a : images in msg
             img_topic_published_mutex_.lock();
             img_topic_published_ = true;
             img_topic_published_mutex_.unlock();
 
-            return; // return from callback because the first set of images are already published
-        } // end if img_topic_published=false
+            return;  // return from callback because the first set of images are already published
+        }  // end if img_topic_published=false
     };
 
     //// airsim lidar callback
-    auto airsim_lidar_cb = [&](auto &msg) {
+    auto airsim_lidar_cb = [&](auto& msg) {
         lidar_data_ = msg->data;
-        if (lidar_data_.empty()) { // return if empty message
+        if (lidar_data_.empty()) {  // return if empty message
             return;
         }
 
@@ -368,15 +380,16 @@ void ROSAirSim::init(std::map<std::string, std::string> &params) {
         if (!lidar_topic_published) {
             // for each image in the message
             for (sc::sensor::AirSimLidarType l : lidar_data_) {
-
                 // Create the topic name
                 std::string lidar_name = boost::algorithm::to_lower_copy(l.lidar_name);
                 std::string topic_name_pub = ros_namespace_ + "/" + lidar_name;
                 // std::string topic_name = "/" + topic_name_pub;
-                lidar_publishers_.push_back(nh_->advertise<sensor_msgs::PointCloud2>(topic_name_pub, 1));
+                lidar_publishers_.push_back(
+                    nh_->advertise<sensor_msgs::PointCloud2>(topic_name_pub, 1));
                 sensor_msgs::PointCloud2 lidar_msg;
                 lidar_msg.header.stamp = ros::Time::now();
-                lidar_msg.header.frame_id = topic_name_pub + "_link";;
+                lidar_msg.header.frame_id = topic_name_pub + "_link";
+                ;
 
                 if (l.lidar_data.point_cloud.size() > 3) {
                     lidar_msg.height = 1;
@@ -391,18 +404,20 @@ void ROSAirSim::init(std::map<std::string, std::string> &params) {
                     for (size_t d = 0; d < lidar_msg.fields.size(); ++d, offset += 4) {
                         lidar_msg.fields[d].offset = offset;
                         lidar_msg.fields[d].datatype = sensor_msgs::PointField::FLOAT32;
-                        lidar_msg.fields[d].count  = 1;
+                        lidar_msg.fields[d].count = 1;
                     }
 
                     lidar_msg.is_bigendian = false;
-                    lidar_msg.point_step = offset; // 4 * num fields
+                    lidar_msg.point_step = offset;  // 4 * num fields
                     lidar_msg.row_step = lidar_msg.point_step * lidar_msg.width;
 
                     lidar_msg.is_dense = true;
                     std::vector<float> data_std = l.lidar_data.point_cloud;
 
-                    const unsigned char* bytes = reinterpret_cast<const unsigned char*>(&data_std[0]);
-                    vector<unsigned char> lidar_msg_data(bytes, bytes + sizeof(float) * data_std.size());
+                    const unsigned char* bytes =
+                        reinterpret_cast<const unsigned char*>(&data_std[0]);
+                    vector<unsigned char> lidar_msg_data(bytes,
+                                                         bytes + sizeof(float) * data_std.size());
                     lidar_msg.data = std::move(lidar_msg_data);
                 }
 
@@ -416,10 +431,12 @@ void ROSAirSim::init(std::map<std::string, std::string> &params) {
                 tf_msg_vec_.clear();
 
                 // Get Vehicle Pose from NED to ENU
-                Eigen::Isometry3f tf_world_vehicle_ENU = get_vehicle_world_pose_from_NED_to_ENU(l.vehicle_pose_world_NED);
+                Eigen::Isometry3f tf_world_vehicle_ENU =
+                    get_vehicle_world_pose_from_NED_to_ENU(l.vehicle_pose_world_NED);
                 // Get pose of lidar in ENU, vehicle frame using Eigen
-                Eigen::Isometry3f tf_vehicle_lidar_ENU = get_sensor_pose_from_worldNED_to_vehicleENU(l.vehicle_pose_world_NED,
-                                                                                                     l.lidar_pose_world_NED);
+                Eigen::Isometry3f tf_vehicle_lidar_ENU =
+                    get_sensor_pose_from_worldNED_to_vehicleENU(l.vehicle_pose_world_NED,
+                                                                l.lidar_pose_world_NED);
 
                 //////////////////////////////////////////////////////////////////////
                 // Update and Publish Transforms
@@ -477,19 +494,19 @@ void ROSAirSim::init(std::map<std::string, std::string> &params) {
 
                 laser_broadcaster_->sendTransform(tf_msg_vec_);
                 tf_msg_vec_.clear();
-            } // end for loop i : imus in msg
+            }  // end for loop i : imus in msg
             lidar_topic_published_mutex_.lock();
             lidar_topic_published_ = true;
             lidar_topic_published_mutex_.unlock();
 
-            return; // return from callback because the first set of images are already published
-        } // end if img_topic_published=false
+            return;  // return from callback because the first set of images are already published
+        }  // end if img_topic_published=false
     };
 
     //// airsim imu callback
-    auto airsim_imu_cb = [&](auto &msg) {
+    auto airsim_imu_cb = [&](auto& msg) {
         imu_data_ = msg->data;
-        if (imu_data_.empty()) { // return if empty message
+        if (imu_data_.empty()) {  // return if empty message
             return;
         }
 
@@ -501,7 +518,6 @@ void ROSAirSim::init(std::map<std::string, std::string> &params) {
         if (!imu_topic_published) {
             // for each image in the message
             for (sc::sensor::AirSimImuType i : imu_data_) {
-
                 // Create the topic name
                 std::string imu_name = boost::algorithm::to_lower_copy(i.imu_name);
                 std::string topic_name_pub = ros_namespace_ + "/" + imu_name;
@@ -532,10 +548,11 @@ void ROSAirSim::init(std::map<std::string, std::string> &params) {
                 tf_msg_vec_.clear();
 
                 // Get Vehicle Pose from NED to ENU
-                Eigen::Isometry3f tf_world_vehicle_ENU = get_vehicle_world_pose_from_NED_to_ENU(i.vehicle_pose_world_NED);
+                Eigen::Isometry3f tf_world_vehicle_ENU =
+                    get_vehicle_world_pose_from_NED_to_ENU(i.vehicle_pose_world_NED);
                 // Get pose of lidar in ENU, vehicle frame using Eigen
-                Eigen::Isometry3f tf_vehicle_imu_ENU = get_sensor_pose_from_worldNED_to_vehicleENU(i.vehicle_pose_world_NED,
-                                                                                                   i.imu_pose_world_NED);
+                Eigen::Isometry3f tf_vehicle_imu_ENU = get_sensor_pose_from_worldNED_to_vehicleENU(
+                    i.vehicle_pose_world_NED, i.imu_pose_world_NED);
 
                 //////////////////////////////////////////////////////////////////////
                 // Update and Publish Transforms
@@ -593,28 +610,30 @@ void ROSAirSim::init(std::map<std::string, std::string> &params) {
 
                 laser_broadcaster_->sendTransform(tf_msg_vec_);
                 tf_msg_vec_.clear();
-            } // end for loop i : imus in msg
+            }  // end for loop i : imus in msg
             imu_topic_published_mutex_.lock();
             imu_topic_published_ = true;
             imu_topic_published_mutex_.unlock();
 
-            return; // return from callback because the first set of images are already published
-        } // end if img_topic_published=false
+            return;  // return from callback because the first set of images are already published
+        }  // end if img_topic_published=false
     };
 
     if (pub_imu_data_) {
         subscribe<std::vector<sensor::AirSimImuType>>("LocalNetwork", "AirSimImu", airsim_imu_cb);
     }
     if (pub_lidar_data_) {
-        subscribe<std::vector<sensor::AirSimLidarType>>("LocalNetwork", "AirSimLidar", airsim_lidar_cb);
+        subscribe<std::vector<sensor::AirSimLidarType>>("LocalNetwork", "AirSimLidar",
+                                                        airsim_lidar_cb);
     }
     if (pub_image_data_) {
-        subscribe<std::vector<sensor::AirSimImageType>>("LocalNetwork", "AirSimImages", airsim_image_cb);
+        subscribe<std::vector<sensor::AirSimImageType>>("LocalNetwork", "AirSimImages",
+                                                        airsim_image_cb);
     }
 }
 
 bool ROSAirSim::step_autonomy(double t, double dt) {
-    ros::spinOnce(); // check for new ROS messages
+    ros::spinOnce();  // check for new ROS messages
     std::vector<geometry_msgs::TransformStamped> tf_msg_vec_;
     tf_msg_vec_.clear();
 
@@ -627,7 +646,6 @@ bool ROSAirSim::step_autonomy(double t, double dt) {
     if (pub_imu_data_ && imu_topic_published) {
         // All image topics should be published
         for (sc::sensor::AirSimImuType i : imu_data_) {
-
             // create string to match using topic name
             std::string imu_name = boost::algorithm::to_lower_copy(i.imu_name);
             std::string topic_name_pub = ros_namespace_ + "/" + imu_name;
@@ -651,22 +669,25 @@ bool ROSAirSim::step_autonomy(double t, double dt) {
                     imu_msg.linear_acceleration.y = i.imu_data.linear_acceleration.y();
                     imu_msg.linear_acceleration.z = i.imu_data.linear_acceleration.z();
                     pub.publish(imu_msg);
-                    break; // break publish for loop after image is published
-                } // end if topic name matches
-            } // end publishers for loop
+                    break;  // break publish for loop after image is published
+                }  // end if topic name matches
+            }  // end publishers for loop
 
             //// for each imu publish a transform
             // Get Vehicle Pose from NED to ENU
-            Eigen::Isometry3f tf_world_vehicle_ENU = get_vehicle_world_pose_from_NED_to_ENU(i.vehicle_pose_world_NED);
+            Eigen::Isometry3f tf_world_vehicle_ENU =
+                get_vehicle_world_pose_from_NED_to_ENU(i.vehicle_pose_world_NED);
             // Get pose of imu in ENU, vehicle frame using Eigen
-            Eigen::Isometry3f tf_vehicle_imu_ENU = get_sensor_pose_from_worldNED_to_vehicleENU(i.vehicle_pose_world_NED,
-                                                                                               i.imu_pose_world_NED);
+            Eigen::Isometry3f tf_vehicle_imu_ENU = get_sensor_pose_from_worldNED_to_vehicleENU(
+                i.vehicle_pose_world_NED, i.imu_pose_world_NED);
 
-            // cout << "If not publishing lidar or image data, publish world frame from imu" << endl;
+            // cout << "If not publishing lidar or image data, publish world frame from imu" <<
+            // endl;
             //////////////////////////////////////////////////////////////////////
             // Update and Publish Transform
             //////////////////////////////////////////////////////////////////////
-            // Publish Vehicle Pose using IMU sensor because it has the least receiving/ processing time
+            // Publish Vehicle Pose using IMU sensor because it has the least receiving/ processing
+            // time
             world_trans_.header.stamp = ros::Time::now();
             world_trans_.header.frame_id = "world";
             if (ros_cartographer_) {
@@ -716,10 +737,9 @@ bool ROSAirSim::step_autonomy(double t, double dt) {
             imu_trans_.transform.rotation.y = tf_vehicle_imu_ENU_rotation.y();
             imu_trans_.transform.rotation.z = tf_vehicle_imu_ENU_rotation.z();
             tf_msg_vec_.push_back(imu_trans_);
-        } // end imus in message for loop
+        }  // end imus in message for loop
     }
 
-
     //// Update LIDAR PointCloud2 ROS message
     //////////////////////////////////////////////////////////////////////
     lidar_topic_published_mutex_.lock();
@@ -729,7 +749,6 @@ bool ROSAirSim::step_autonomy(double t, double dt) {
     if (pub_lidar_data_ && lidar_topic_published) {
         // All image topics should be published
         for (sc::sensor::AirSimLidarType l : lidar_data_) {
-
             // Create the topic name
             std::string lidar_name = boost::algorithm::to_lower_copy(l.lidar_name);
             std::string topic_name_pub = ros_namespace_ + "/" + lidar_name;
@@ -755,33 +774,37 @@ bool ROSAirSim::step_autonomy(double t, double dt) {
                         for (size_t d = 0; d < lidar_msg.fields.size(); ++d, offset += 4) {
                             lidar_msg.fields[d].offset = offset;
                             lidar_msg.fields[d].datatype = sensor_msgs::PointField::FLOAT32;
-                            lidar_msg.fields[d].count  = 1;
+                            lidar_msg.fields[d].count = 1;
                         }
 
                         lidar_msg.is_bigendian = false;
-                        lidar_msg.point_step = offset; // 4 * num fields
+                        lidar_msg.point_step = offset;  // 4 * num fields
                         lidar_msg.row_step = lidar_msg.point_step * lidar_msg.width;
 
                         lidar_msg.is_dense = true;
                         std::vector<float> data_std = l.lidar_data.point_cloud;
 
-                        const unsigned char* bytes = reinterpret_cast<const unsigned char*>(&data_std[0]);
-                        vector<unsigned char> lidar_msg_data(bytes, bytes + sizeof(float) * data_std.size());
+                        const unsigned char* bytes =
+                            reinterpret_cast<const unsigned char*>(&data_std[0]);
+                        vector<unsigned char> lidar_msg_data(
+                            bytes, bytes + sizeof(float) * data_std.size());
                         lidar_msg.data = std::move(lidar_msg_data);
                     }
                     pub.publish(lidar_msg);
-                    break; // break publish for loop after lidar data is published
-                } // end if topic name matches
-            } // end publishers for loop
+                    break;  // break publish for loop after lidar data is published
+                }  // end if topic name matches
+            }  // end publishers for loop
 
             //// publish a transform for each lidar
             // Get Vehicle Pose from NED to ENU
-            Eigen::Isometry3f tf_world_vehicle_ENU = get_vehicle_world_pose_from_NED_to_ENU(l.vehicle_pose_world_NED);
+            Eigen::Isometry3f tf_world_vehicle_ENU =
+                get_vehicle_world_pose_from_NED_to_ENU(l.vehicle_pose_world_NED);
             // Get pose of lidar in ENU, vehicle frame using Eigen
-            Eigen::Isometry3f tf_vehicle_lidar_ENU = get_sensor_pose_from_worldNED_to_vehicleENU(l.vehicle_pose_world_NED,
-                                                                                                 l.lidar_pose_world_NED);
+            Eigen::Isometry3f tf_vehicle_lidar_ENU = get_sensor_pose_from_worldNED_to_vehicleENU(
+                l.vehicle_pose_world_NED, l.lidar_pose_world_NED);
 
-            // If not publishing imu, publish world frame from lidar -- 2nd least receiving/ processing time.
+            // If not publishing imu, publish world frame from lidar -- 2nd least receiving/
+            // processing time.
             if (!pub_imu_data_) {
                 // cout << "If not publishing imu data, publish world frame from lidar" << endl;
                 //////////////////////////////////////////////////////////////////////
@@ -836,10 +859,9 @@ bool ROSAirSim::step_autonomy(double t, double dt) {
             lidar_trans_.transform.rotation.y = tf_vehicle_lidar_ENU_rotation.y();
             lidar_trans_.transform.rotation.z = tf_vehicle_lidar_ENU_rotation.z();
             tf_msg_vec_.push_back(lidar_trans_);
-        } // end lidars in message for loop
+        }  // end lidars in message for loop
     }
 
-
     //// Update Images
     //////////////////////////////////////////////////////////////////////
     img_topic_published_mutex_.lock();
@@ -851,14 +873,16 @@ bool ROSAirSim::step_autonomy(double t, double dt) {
         for (sc::sensor::AirSimImageType a : image_data_) {
             // create string to match using topic name
             std::string camera_name = boost::algorithm::to_lower_copy(a.camera_config.cam_name);
-            std::string image_type_name = boost::algorithm::to_lower_copy(a.camera_config.img_type_name);
-            std::string topic_name_pub = ros_namespace_ + "/camera/" + camera_name + "/" + image_type_name;
+            std::string image_type_name =
+                boost::algorithm::to_lower_copy(a.camera_config.img_type_name);
+            std::string topic_name_pub =
+                ros_namespace_ + "/camera/" + camera_name + "/" + image_type_name;
             // std::string info_topic_name_pub = topic_name_pub + "/info";
-            topic_name_pub += + "/raw";
+            topic_name_pub += +"/raw";
             std::string topic_name_match = "/" + topic_name_pub;
 
-            std_msgs::Header header; // empty header
-            header.stamp = ros::Time::now(); // time
+            std_msgs::Header header;          // empty header
+            header.stamp = ros::Time::now();  // time
             header.frame_id = topic_name_pub + "_link";
             sensor_msgs::ImagePtr img_msg;
 
@@ -867,58 +891,62 @@ bool ROSAirSim::step_autonomy(double t, double dt) {
                 for (auto pub : img_publishers_) {
                     // If the topic publisher exists, publish to topic
                     if (pub.getTopic() == topic_name_match) {
-                        // Depth Images (Depth Perspective and Depth Planner) come in as 1 channel float arrays
+                        // Depth Images (Depth Perspective and Depth Planner) come in as 1 channel
+                        // float arrays
                         if (a.camera_config.pixels_as_float) {
                             img_msg = cv_bridge::CvImage(header, "", a.img).toImageMsg();
                         } else {
                             img_msg = cv_bridge::CvImage(header, "rgb8", a.img).toImageMsg();
                         }
                         pub.publish(img_msg);
-                        break; // break publish for loop after image is published
-                    } // end if topic name matches
-                } // end publishers for loop
+                        break;  // break publish for loop after image is published
+                    }  // end if topic name matches
+                }  // end publishers for loop
             } else {
                 for (auto pub : trans_img_publishers_) {
                     // If the topic publisher exists, publish to topic
                     if (pub.getTopic() == topic_name_match) {
-                        // Depth Images (Depth Perspective and Depth Planner) come in as 1 channel float arrays
+                        // Depth Images (Depth Perspective and Depth Planner) come in as 1 channel
+                        // float arrays
                         if (a.camera_config.pixels_as_float) {
                             img_msg = cv_bridge::CvImage(header, "", a.img).toImageMsg();
                         } else {
                             img_msg = cv_bridge::CvImage(header, "rgb8", a.img).toImageMsg();
                         }
                         pub.publish(img_msg);
-                        break; // break publish for loop after image is published
-                    } // end if topic name matches
-                } // end publishers for loop
+                        break;  // break publish for loop after image is published
+                    }  // end if topic name matches
+                }  // end publishers for loop
             }
 
             // draw image
             if (show_camera_images_) {
-                std::string window_name = a.vehicle_name + "_" + a.camera_config.cam_name + "_" + a.camera_config.img_type_name;
+                std::string window_name = a.vehicle_name + "_" + a.camera_config.cam_name + "_"
+                                          + a.camera_config.img_type_name;
                 if (a.camera_config.pixels_as_float) {
                     cv::Mat tempImage;
-                    a.img.convertTo(tempImage, CV_32FC1, 1.f/255);
-                    // cv::normalize(a.img, tempImage, 0, 1, cv::NORM_MINMAso use if to determine.X);
-                    // cout << tempImage << endl;
+                    a.img.convertTo(tempImage, CV_32FC1, 1.f / 255);
+                    // cv::normalize(a.img, tempImage, 0, 1, cv::NORM_MINMAso use if to
+                    // determine.X); cout << tempImage << endl;
                     cv::imshow(window_name, tempImage);
                 } else {
                     // other image types are int 0-255.
                     if (a.img.channels() == 4) {
                         cout << "image channels: " << a.img.channels() << endl;
-                        cout << "Warning: Old AirSim Linux Asset Environments have 4 channels. Color images will not display correctly." << endl;
+                        cout << "Warning: Old AirSim Linux Asset Environments have 4 channels. "
+                                "Color images will not display correctly."
+                             << endl;
                         cout << "Warning: Use Asset Environment versions Linux-v1.3.1+." << endl;
                         cv::Mat tempImage;
-                        cv::cvtColor(a.img , tempImage, CV_RGBA2RGB);
+                        cv::cvtColor(a.img, tempImage, CV_RGBA2RGB);
                         cv::imshow(window_name, tempImage);
                     } else {
                         cv::imshow(window_name, a.img);
                     }
                 }
                 cv::waitKey(1);
-            } // end draw image
-        } // end images in message for loop
-
+            }  // end draw image
+        }  // end images in message for loop
 
         // for each camera_name publish a transform
         // cout << "camera_names length: " << camera_names_.size() << endl;
@@ -927,14 +955,17 @@ bool ROSAirSim::step_autonomy(double t, double dt) {
                 if (a.camera_config.cam_name == cam_name) {
                     // cout << "publishing transform for: " << cam_name << endl;
                     // create string to match using topic name
-                    std::string camera_name = boost::algorithm::to_lower_copy(a.camera_config.cam_name);
+                    std::string camera_name =
+                        boost::algorithm::to_lower_copy(a.camera_config.cam_name);
                     std::string cam_topic_name = ros_namespace_ + "/camera/" + camera_name;
 
                     // Get Vehicle Pose from NED to ENU
-                    Eigen::Isometry3f tf_world_vehicle_ENU = get_vehicle_world_pose_from_NED_to_ENU(a.vehicle_pose_world_NED);
+                    Eigen::Isometry3f tf_world_vehicle_ENU =
+                        get_vehicle_world_pose_from_NED_to_ENU(a.vehicle_pose_world_NED);
                     // Get pose of camera in ENU, vehicle frame using Eigen
-                    Eigen::Isometry3f tf_vehicle_camera_ENU = get_sensor_pose_from_worldNED_to_vehicleENU(a.vehicle_pose_world_NED,
-                                                                                                          a.camera_pose_world_NED);
+                    Eigen::Isometry3f tf_vehicle_camera_ENU =
+                        get_sensor_pose_from_worldNED_to_vehicleENU(a.vehicle_pose_world_NED,
+                                                                    a.camera_pose_world_NED);
                     // cout << "publishing transform for: " << cam_name << endl;
 
                     // If not publishing lidar or imu data, publish world frame from images
@@ -949,10 +980,14 @@ bool ROSAirSim::step_autonomy(double t, double dt) {
                         } else {
                             world_trans_.child_frame_id = ros_namespace_ + "/base_link";
                         }
-                        world_trans_.transform.translation.x = tf_world_vehicle_ENU.translation().x();
-                        world_trans_.transform.translation.y = tf_world_vehicle_ENU.translation().y();
-                        world_trans_.transform.translation.z = tf_world_vehicle_ENU.translation().z();
-                        Eigen::Quaternionf tf_vehicle_pose_rotation(tf_world_vehicle_ENU.rotation());
+                        world_trans_.transform.translation.x =
+                            tf_world_vehicle_ENU.translation().x();
+                        world_trans_.transform.translation.y =
+                            tf_world_vehicle_ENU.translation().y();
+                        world_trans_.transform.translation.z =
+                            tf_world_vehicle_ENU.translation().z();
+                        Eigen::Quaternionf tf_vehicle_pose_rotation(
+                            tf_world_vehicle_ENU.rotation());
                         world_trans_.transform.rotation.x = tf_vehicle_pose_rotation.x();
                         world_trans_.transform.rotation.y = tf_vehicle_pose_rotation.y();
                         world_trans_.transform.rotation.z = tf_vehicle_pose_rotation.z();
@@ -981,29 +1016,30 @@ bool ROSAirSim::step_autonomy(double t, double dt) {
                     image_trans_.child_frame_id = cam_topic_name + "_link";
                     image_trans_.header.stamp = ros::Time::now();
 
-                    // Pose in image_data.pose is in NED, but scrimmage is in ENU so use the conversion done in AirSimSensor
-                    // Position
+                    // Pose in image_data.pose is in NED, but scrimmage is in ENU so use the
+                    // conversion done in AirSimSensor Position
                     image_trans_.transform.translation.x = tf_vehicle_camera_ENU.translation().x();
                     image_trans_.transform.translation.y = tf_vehicle_camera_ENU.translation().y();
                     image_trans_.transform.translation.z = tf_vehicle_camera_ENU.translation().z();
                     // Orientation
-                    Eigen::Quaternionf tf_vehicle_camera_ENU_rotation(tf_vehicle_camera_ENU.rotation());
+                    Eigen::Quaternionf tf_vehicle_camera_ENU_rotation(
+                        tf_vehicle_camera_ENU.rotation());
                     image_trans_.transform.rotation.w = tf_vehicle_camera_ENU_rotation.w();
                     image_trans_.transform.rotation.x = tf_vehicle_camera_ENU_rotation.x();
                     image_trans_.transform.rotation.y = tf_vehicle_camera_ENU_rotation.y();
                     image_trans_.transform.rotation.z = tf_vehicle_camera_ENU_rotation.z();
                     tf_msg_vec_.push_back(image_trans_);
 
-                    break; // break the image in msg for loop if camera_name is found and move to next camera_name
+                    break;  // break the image in msg for loop if camera_name is found and move to
+                            // next camera_name
                 }
             }
         }
     }
 
-
     // If none of the sensor msgs are being published use parent_->state_truth() vehicle pose
     if (!pub_image_data_ && !pub_lidar_data_ && !pub_imu_data_) {
-        sc::StatePtr &state = parent_->state_truth();
+        sc::StatePtr& state = parent_->state_truth();
 
         world_trans_.header.stamp = ros::Time::now();
         world_trans_.header.frame_id = "world";
@@ -1042,5 +1078,5 @@ bool ROSAirSim::step_autonomy(double t, double dt) {
 
     return true;
 }
-} // namespace autonomy
-} // namespace scrimmage
+}  // namespace autonomy
+}  // namespace scrimmage
diff --git a/src/plugins/autonomy/ROSAutonomy/ROSAutonomy.cpp b/src/plugins/autonomy/ROSAutonomy/ROSAutonomy.cpp
index e15972fde4..e40769ec90 100644
--- a/src/plugins/autonomy/ROSAutonomy/ROSAutonomy.cpp
+++ b/src/plugins/autonomy/ROSAutonomy/ROSAutonomy.cpp
@@ -30,17 +30,14 @@
  *
  */
 
-
 #include <rosgraph_msgs/Clock.h>
-
-#include <scrimmage/plugin_manager/RegisterPlugin.h>
 #include <scrimmage/entity/Entity.h>
+#include <scrimmage/math/Angles.h>
 #include <scrimmage/math/State.h>
-
+#include <scrimmage/parse/ParseUtils.h>
+#include <scrimmage/plugin_manager/RegisterPlugin.h>
 #include <scrimmage/plugins/autonomy/ROSAutonomy/ROSAutonomy.h>
 #include <scrimmage/plugins/sensor/RayTrace/RayTrace.h>
-#include <scrimmage/parse/ParseUtils.h>
-#include <scrimmage/math/Angles.h>
 
 #include <iostream>
 
@@ -54,9 +51,10 @@ REGISTER_PLUGIN(scrimmage::Autonomy, scrimmage::autonomy::ROSAutonomy, ROSAutono
 namespace scrimmage {
 namespace autonomy {
 
-ROSAutonomy::ROSAutonomy() {}
+ROSAutonomy::ROSAutonomy() {
+}
 
-void ROSAutonomy::init(std::map<std::string, std::string> &params) {
+void ROSAutonomy::init(std::map<std::string, std::string>& params) {
     speed_idx_ = vars_.declare(VariableIO::Type::speed, VariableIO::Direction::Out);
     turn_rate_idx_ = vars_.declare(VariableIO::Type::turn_rate, VariableIO::Direction::Out);
     pitch_rate_idx_ = vars_.declare(VariableIO::Type::pitch_rate, VariableIO::Direction::Out);
@@ -76,7 +74,8 @@ void ROSAutonomy::init(std::map<std::string, std::string> &params) {
     cmd_vel_sub_ = nh_->subscribe(ros_namespace_ + "/cmd_vel", 1, &ROSAutonomy::cmd_vel_cb, this);
     odom_pub_ = nh_->advertise<nav_msgs::Odometry>(ros_namespace_ + "/odom", 1);
     base_scan_pub_ = nh_->advertise<sensor_msgs::LaserScan>(ros_namespace_ + "/base_scan", 1);
-    base_pose_truth_pub = nh_->advertise<nav_msgs::Odometry>(ros_namespace_ + "/base_pose_ground_truth", 1);
+    base_pose_truth_pub =
+        nh_->advertise<nav_msgs::Odometry>(ros_namespace_ + "/base_pose_ground_truth", 1);
 
     odom_broadcaster_ = std::make_shared<tf::TransformBroadcaster>();
     laser_broadcaster_ = std::make_shared<tf::TransformBroadcaster>();
@@ -89,10 +88,11 @@ void ROSAutonomy::init(std::map<std::string, std::string> &params) {
     laser_trans_.transform.translation.x = parent_->sensors()["RayTrace0"]->transform()->pos()(0);
     laser_trans_.transform.translation.y = parent_->sensors()["RayTrace0"]->transform()->pos()(1);
     laser_trans_.transform.translation.z = parent_->sensors()["RayTrace0"]->transform()->pos()(2);
-    geometry_msgs::Quaternion laser_quat = tf::createQuaternionMsgFromYaw(parent_->sensors()["RayTrace0"]->transform()->quat().yaw());
+    geometry_msgs::Quaternion laser_quat =
+        tf::createQuaternionMsgFromYaw(parent_->sensors()["RayTrace0"]->transform()->quat().yaw());
     laser_trans_.transform.rotation = laser_quat;
 
-    auto pc_cb = [&] (scrimmage::MessagePtr<sc::sensor::RayTrace::PointCloud> msg) {
+    auto pc_cb = [&](scrimmage::MessagePtr<sc::sensor::RayTrace::PointCloud> msg) {
         pcl_ = msg->data;
     };
     subscribe<sc::sensor::RayTrace::PointCloud>("LocalNetwork", "RayTrace/pointcloud", pc_cb);
@@ -107,7 +107,7 @@ bool ROSAutonomy::step_autonomy(double t, double dt) {
     // Update ROS time
     ros::Time ros_time = ros::Time::now();
 
-    ros::spinOnce(); // check for new ROS messages
+    ros::spinOnce();  // check for new ROS messages
 
     // Convert scrimmage point cloud into ROS laser scan message
     // double fov_half = pcl_.num_rays_horiz * pcl_.angle_res_horiz / 2.0;
@@ -123,7 +123,7 @@ bool ROSAutonomy::step_autonomy(double t, double dt) {
     laser_msg.intensities.resize(pcl_.points.size());
 
     int i = 0;
-    for (sc::sensor::RayTrace::PCPoint &p : pcl_.points) {
+    for (sc::sensor::RayTrace::PCPoint& p : pcl_.points) {
         laser_msg.ranges[i] = p.point.norm();
         laser_msg.intensities[i] = p.intensity;
         i++;
@@ -159,7 +159,7 @@ bool ROSAutonomy::step_autonomy(double t, double dt) {
     odom.twist.twist.linear.x = state_->vel()(0);
     odom.twist.twist.linear.y = state_->vel()(1);
     odom.twist.twist.linear.z = state_->vel()(2);
-    odom.twist.twist.angular.z = 0; // TODO
+    odom.twist.twist.angular.z = 0;  // TODO
 
     odom_pub_.publish(odom);
 
@@ -181,5 +181,5 @@ void ROSAutonomy::cmd_vel_cb(const geometry_msgs::Twist::ConstPtr& msg) {
     cmd_vel_ = *msg;
 }
 
-} // namespace autonomy
-} // namespace scrimmage
+}  // namespace autonomy
+}  // namespace scrimmage
diff --git a/src/plugins/autonomy/ROSControl/ROSControl.cpp b/src/plugins/autonomy/ROSControl/ROSControl.cpp
index ed345c84df..315658632c 100644
--- a/src/plugins/autonomy/ROSControl/ROSControl.cpp
+++ b/src/plugins/autonomy/ROSControl/ROSControl.cpp
@@ -30,17 +30,14 @@
  *
  */
 
-
 #include <rosgraph_msgs/Clock.h>
-
-#include <scrimmage/plugin_manager/RegisterPlugin.h>
 #include <scrimmage/entity/Entity.h>
+#include <scrimmage/math/Angles.h>
 #include <scrimmage/math/State.h>
-
+#include <scrimmage/parse/ParseUtils.h>
+#include <scrimmage/plugin_manager/RegisterPlugin.h>
 #include <scrimmage/plugins/autonomy/ROSControl/ROSControl.h>
 #include <scrimmage/plugins/sensor/RayTrace/RayTrace.h>
-#include <scrimmage/parse/ParseUtils.h>
-#include <scrimmage/math/Angles.h>
 
 #include <boost/numeric/odeint.hpp>
 
@@ -52,9 +49,10 @@ REGISTER_PLUGIN(scrimmage::Autonomy, scrimmage::autonomy::ROSControl, ROSControl
 namespace scrimmage {
 namespace autonomy {
 
-ROSControl::ROSControl() {}
+ROSControl::ROSControl() {
+}
 
-void ROSControl::init(std::map<std::string, std::string> &params) {
+void ROSControl::init(std::map<std::string, std::string>& params) {
     if (!ros::isInitialized()) {
         int argc = 0;
         // scrimmage handles it's own SIGINT/SIGTERM shutdown in main.cpp
@@ -74,7 +72,7 @@ void ROSControl::init(std::map<std::string, std::string> &params) {
 
     desired_state_->vel() = Eigen::Vector3d::UnitX() * 0;
     desired_state_->quat().set(0, 0, state_->quat().yaw());
-    desired_state_->pos() = Eigen::Vector3d::UnitZ()*state_->pos()(2);
+    desired_state_->pos() = Eigen::Vector3d::UnitZ() * state_->pos()(2);
 
     //  ofs_.open("/home/syllogismrxs/tmp/data.csv",
     //            std::ofstream::out | std::ofstream::trunc);
@@ -90,15 +88,14 @@ void ROSControl::zero_ctrls() {
     cmd_vel_.angular.z = 0;
 }
 
-void ROSControl::ctrl_filter(const std::vector<double> &x ,
-                             std::vector<double> &dxdt , double t) {
+void ROSControl::ctrl_filter(const std::vector<double>& x, std::vector<double>& dxdt, double t) {
     dxdt[0] = x[1];
     dxdt[1] = -x[2];
     dxdt[2] = 0;
 }
 
 bool ROSControl::step_autonomy(double t, double dt) {
-    ros::spinOnce(); // check for new ROS messages
+    ros::spinOnce();  // check for new ROS messages
 
     // auto sys = std::bind(&ROSControl::ctrl_filter, this, pl::_1, pl::_2, pl::_3);
     // boost::numeric::odeint::runge_kutta4<std::vector<double>> stepper;
@@ -115,8 +112,8 @@ bool ROSControl::step_autonomy(double t, double dt) {
     // Need to saturate state variables before model runs
 
     desired_state_->vel()(0) = 30;
-    desired_state_->vel()(1) = 10*cmd_vel_.angular.z;
-    desired_state_->vel()(2) = 10*cmd_vel_.linear.x;
+    desired_state_->vel()(1) = 10 * cmd_vel_.angular.z;
+    desired_state_->vel()(2) = 10 * cmd_vel_.linear.x;
 
     zero_ctrls();
 
@@ -126,5 +123,5 @@ bool ROSControl::step_autonomy(double t, double dt) {
 void ROSControl::cmd_vel_cb(const geometry_msgs::Twist::ConstPtr& msg) {
     cmd_vel_ = *msg;
 }
-} // namespace autonomy
-} // namespace scrimmage
+}  // namespace autonomy
+}  // namespace scrimmage
diff --git a/src/plugins/autonomy/ScrimmageOpenAIAutonomy/ActorFunc.cpp b/src/plugins/autonomy/ScrimmageOpenAIAutonomy/ActorFunc.cpp
index ed4cdcf9b9..bfbc72f209 100644
--- a/src/plugins/autonomy/ScrimmageOpenAIAutonomy/ActorFunc.cpp
+++ b/src/plugins/autonomy/ScrimmageOpenAIAutonomy/ActorFunc.cpp
@@ -32,7 +32,6 @@
 
 #include <pybind11/pybind11.h>
 #include <pybind11/stl.h>
-
 #include <scrimmage/entity/Entity.h>
 #include <scrimmage/parse/ParseUtils.h>
 #include <scrimmage/plugins/autonomy/ScrimmageOpenAIAutonomy/ActorFunc.h>
@@ -40,9 +39,8 @@
 #include <scrimmage/plugins/autonomy/ScrimmageOpenAIAutonomy/OpenAIObservations.h>
 #include <scrimmage/plugins/autonomy/ScrimmageOpenAIAutonomy/ScrimmageOpenAIAutonomy.h>
 
-#include <iostream>
-
 #include <boost/range/algorithm/copy.hpp>
+#include <iostream>
 
 using std::cout;
 using std::endl;
@@ -53,14 +51,10 @@ namespace br = boost::range;
 namespace scrimmage {
 namespace autonomy {
 
-std::tuple<OpenAIActions, OpenAIObservations, pybind11::object>
-init_actor_func(
-        std::vector<std::shared_ptr<ScrimmageOpenAIAutonomy>> autonomies,
-        const std::map<std::string, std::string> &params,
-        CombineActors combine_actors,
-        UseGlobalSensor global_sensor,
-        bool grpc_mode) {
-
+std::tuple<OpenAIActions, OpenAIObservations, pybind11::object> init_actor_func(
+    std::vector<std::shared_ptr<ScrimmageOpenAIAutonomy>> autonomies,
+    const std::map<std::string, std::string>& params, CombineActors combine_actors,
+    UseGlobalSensor global_sensor, bool grpc_mode) {
     if (autonomies.empty()) {
         return std::make_tuple(OpenAIActions(), OpenAIObservations(), pybind11::none());
     }
@@ -97,10 +91,9 @@ init_actor_func(
         py::object m = py::module::import(module_str.c_str());
         actor_init_func = m.attr(actor_init_func_str.c_str());
 
-        actor_func = actor_init_func(
-            actions.action_space, observations.observation, params);
+        actor_func = actor_init_func(actions.action_space, observations.observation, params);
     }
     return std::make_tuple(actions, observations, actor_func);
 }
-} // namespace autonomy
-} // namespace scrimmage
+}  // namespace autonomy
+}  // namespace scrimmage
diff --git a/src/plugins/autonomy/ScrimmageOpenAIAutonomy/OpenAIActions.cpp b/src/plugins/autonomy/ScrimmageOpenAIAutonomy/OpenAIActions.cpp
index 4c06701803..4afdb904c7 100644
--- a/src/plugins/autonomy/ScrimmageOpenAIAutonomy/OpenAIActions.cpp
+++ b/src/plugins/autonomy/ScrimmageOpenAIAutonomy/OpenAIActions.cpp
@@ -30,9 +30,8 @@
  *
  */
 
-#include <pybind11/pybind11.h>
 #include <pybind11/numpy.h>
-
+#include <pybind11/pybind11.h>
 #include <scrimmage/plugins/autonomy/ScrimmageOpenAIAutonomy/OpenAIActions.h>
 #include <scrimmage/plugins/autonomy/ScrimmageOpenAIAutonomy/OpenAIUtils.h>
 #include <scrimmage/plugins/autonomy/ScrimmageOpenAIAutonomy/ScrimmageOpenAIAutonomy.h>
@@ -47,12 +46,13 @@ using std::endl;
 namespace scrimmage {
 namespace autonomy {
 
-OpenAIActions::OpenAIActions() :
-    tuple_space_(get_gym_space("Tuple")),
-    box_space_(get_gym_space("Box")),
-    asarray_(py::module::import("numpy").attr("asarray")) {}
+OpenAIActions::OpenAIActions()
+    : tuple_space_(get_gym_space("Tuple")),
+      box_space_(get_gym_space("Box")),
+      asarray_(py::module::import("numpy").attr("asarray")) {
+}
 
-std::vector<std::shared_ptr<ScrimmageOpenAIAutonomy>> &OpenAIActions::ext_ctrl_vec() {
+std::vector<std::shared_ptr<ScrimmageOpenAIAutonomy>>& OpenAIActions::ext_ctrl_vec() {
     return ext_ctrl_vec_;
 }
 
@@ -65,20 +65,18 @@ void OpenAIActions::create_action_space(bool combine_actors) {
         py::list continuous_minima;
         py::list continuous_maxima;
 
-        for (auto &a : ext_ctrl_vec_) {
+        for (auto& a : ext_ctrl_vec_) {
             a->set_environment();
             to_discrete(a->action_space.discrete_count, discrete_count);
             to_continuous(a->action_space.continuous_extrema, continuous_minima, continuous_maxima);
         }
 
-        action_space =
-            create_space(discrete_count, continuous_minima, continuous_maxima);
+        action_space = create_space(discrete_count, continuous_minima, continuous_maxima);
 
     } else {
-
         py::list action_spaces;
 
-        for (auto &a : ext_ctrl_vec_) {
+        for (auto& a : ext_ctrl_vec_) {
             py::list discrete_count;
             py::list continuous_minima;
             py::list continuous_maxima;
@@ -87,8 +85,7 @@ void OpenAIActions::create_action_space(bool combine_actors) {
             to_discrete(a->action_space.discrete_count, discrete_count);
             to_continuous(a->action_space.continuous_extrema, continuous_minima, continuous_maxima);
 
-            auto space =
-                create_space(discrete_count, continuous_minima, continuous_maxima);
+            auto space = create_space(discrete_count, continuous_minima, continuous_maxima);
             action_spaces.append(space);
         }
 
@@ -97,8 +94,7 @@ void OpenAIActions::create_action_space(bool combine_actors) {
     }
 }
 
-void OpenAIActions::distribute_action(pybind11::object action,
-                                      bool combine_actors) {
+void OpenAIActions::distribute_action(pybind11::object action, bool combine_actors) {
     py::array_t<int> disc_actions;
     py::array_t<double> cont_actions;
     int* disc_action_data = nullptr;
@@ -124,7 +120,7 @@ void OpenAIActions::distribute_action(pybind11::object action,
         }
     };
 
-    auto put_action = [&](int &idx, size_t from_sz, auto *from_data, auto &to_vec) {
+    auto put_action = [&](int& idx, size_t from_sz, auto* from_data, auto& to_vec) {
         if (to_vec.size() != from_sz) {
             to_vec.resize(from_sz);
         }
@@ -138,18 +134,18 @@ void OpenAIActions::distribute_action(pybind11::object action,
         }
     };
 
-    auto put_actions = [&](auto a, int &disc_action_idx, int &cont_action_idx) {
-        put_action(disc_action_idx, a->action_space.discrete_count.size(),
-                   disc_action_data, a->action.discrete);
-        put_action(cont_action_idx, a->action_space.continuous_extrema.size(),
-                   cont_action_data, a->action.continuous);
+    auto put_actions = [&](auto a, int& disc_action_idx, int& cont_action_idx) {
+        put_action(disc_action_idx, a->action_space.discrete_count.size(), disc_action_data,
+                   a->action.discrete);
+        put_action(cont_action_idx, a->action_space.continuous_extrema.size(), cont_action_data,
+                   a->action.continuous);
     };
 
     if ((ext_ctrl_vec_.size() == 1 || combine_actors)) {
         update_action_lists(action_space, action);
         int disc_action_idx = 0;
         int cont_action_idx = 0;
-        for (auto &a : ext_ctrl_vec_) {
+        for (auto& a : ext_ctrl_vec_) {
             put_actions(a, disc_action_idx, cont_action_idx);
         }
 
@@ -167,6 +163,5 @@ void OpenAIActions::distribute_action(pybind11::object action,
     }
 }
 
-
-} // namespace autonomy
-} // namespace scrimmage
+}  // namespace autonomy
+}  // namespace scrimmage
diff --git a/src/plugins/autonomy/ScrimmageOpenAIAutonomy/OpenAIObservations.cpp b/src/plugins/autonomy/ScrimmageOpenAIAutonomy/OpenAIObservations.cpp
index 0dd8b5cbc2..65e2fc4251 100644
--- a/src/plugins/autonomy/ScrimmageOpenAIAutonomy/OpenAIObservations.cpp
+++ b/src/plugins/autonomy/ScrimmageOpenAIAutonomy/OpenAIObservations.cpp
@@ -30,16 +30,15 @@
  *
  */
 
-#include <pybind11/pybind11.h>
 #include <pybind11/numpy.h>
-
+#include <pybind11/pybind11.h>
+#include <scrimmage/plugins/autonomy/ScrimmageOpenAIAutonomy/OpenAIObservations.h>
 #include <scrimmage/plugins/autonomy/ScrimmageOpenAIAutonomy/OpenAIUtils.h>
 #include <scrimmage/plugins/sensor/ScrimmageOpenAISensor/ScrimmageOpenAISensor.h>
-#include <scrimmage/plugins/autonomy/ScrimmageOpenAIAutonomy/OpenAIObservations.h>
 
-#include <boost/range/algorithm/transform.hpp>
 #include <boost/range/adaptor/filtered.hpp>
 #include <boost/range/adaptor/map.hpp>
+#include <boost/range/algorithm/transform.hpp>
 
 namespace py = pybind11;
 namespace br = boost::range;
@@ -51,14 +50,13 @@ using std::endl;
 namespace scrimmage {
 namespace autonomy {
 
-OpenAIObservations::OpenAIObservations() :
-    tuple_space_(get_gym_space("Tuple")),
-    box_space_(get_gym_space("Box")) {}
+OpenAIObservations::OpenAIObservations()
+    : tuple_space_(get_gym_space("Tuple")), box_space_(get_gym_space("Box")) {
+}
 
 pybind11::object OpenAIObservations::update_observation(size_t num_entities,
                                                         bool static_obs_space) {
-
-    auto call_get_obs = [&](auto *data, uint32_t &beg_idx, auto sensor, int obs_size) {
+    auto call_get_obs = [&](auto* data, uint32_t& beg_idx, auto sensor, int obs_size) {
         if (sensor->parent() == nullptr) {
             return;
         }
@@ -91,13 +89,13 @@ pybind11::object OpenAIObservations::update_observation(size_t num_entities,
         std::tie(disc_obs, cont_obs) = init_arrays(observation_space, observation);
         uint32_t disc_beg_idx = 0;
         uint32_t cont_beg_idx = 0;
-        int* r_disc = static_cast<int *>(disc_obs.request().ptr);
-        double* r_cont = static_cast<double *>(cont_obs.request().ptr);
+        int* r_disc = static_cast<int*>(disc_obs.request().ptr);
+        double* r_cont = static_cast<double*>(cont_obs.request().ptr);
 
         bool done = false;
-        for (auto &v : ext_sensor_vec_) {
+        for (auto& v : ext_sensor_vec_) {
             if (done) break;
-            for (auto &s : v) {
+            for (auto& s : v) {
                 auto obs_space = s->observation_space;
                 call_get_obs(r_disc, disc_beg_idx, s, obs_space.discrete_count.size());
                 call_get_obs(r_cont, cont_beg_idx, s, obs_space.continuous_extrema.size());
@@ -110,8 +108,7 @@ pybind11::object OpenAIObservations::update_observation(size_t num_entities,
         }
 
     } else {
-        py::list observation_space_list =
-                observation_space.attr("spaces").cast<py::list>();
+        py::list observation_space_list = observation_space.attr("spaces").cast<py::list>();
         py::list observation_list = observation.cast<py::list>();
 
         // Number of entities has shrunk
@@ -128,10 +125,10 @@ pybind11::object OpenAIObservations::update_observation(size_t num_entities,
             std::tie(disc_obs, cont_obs) = init_arrays(indiv_space, indiv_obs);
             uint32_t disc_beg_idx = 0;
             uint32_t cont_beg_idx = 0;
-            int* r_disc = static_cast<int *>(disc_obs.request().ptr);
-            double* r_cont = static_cast<double *>(cont_obs.request().ptr);
+            int* r_disc = static_cast<int*>(disc_obs.request().ptr);
+            double* r_cont = static_cast<double*>(cont_obs.request().ptr);
 
-            for (auto &s : ext_sensor_vec_[i]) {
+            for (auto& s : ext_sensor_vec_[i]) {
                 auto obs_space = s->observation_space;
                 call_get_obs(r_disc, disc_beg_idx, s, obs_space.discrete_count.size());
                 call_get_obs(r_cont, cont_beg_idx, s, obs_space.continuous_extrema.size());
@@ -141,11 +138,8 @@ pybind11::object OpenAIObservations::update_observation(size_t num_entities,
     return observation;
 }
 
-void OpenAIObservations::create_observation_space(size_t num_entities,
-                                                  bool static_obs_space) {
-    auto create_obs = [&](py::list &discrete_count,
-                          py::list &continuous_maxima) -> py::object {
-
+void OpenAIObservations::create_observation_space(size_t num_entities, bool static_obs_space) {
+    auto create_obs = [&](py::list& discrete_count, py::list& continuous_maxima) -> py::object {
         int len_discrete = py::len(discrete_count);
         int len_continuous = py::len(continuous_maxima);
 
@@ -173,12 +167,13 @@ void OpenAIObservations::create_observation_space(size_t num_entities,
         py::list continuous_maxima;
 
         bool done = false;
-        for (auto &v : ext_sensor_vec_) {
+        for (auto& v : ext_sensor_vec_) {
             if (done) break;
-            for (auto &s : v) {
+            for (auto& s : v) {
                 s->set_observation_space();
                 to_discrete(s->observation_space.discrete_count, discrete_count);
-                to_continuous(s->observation_space.continuous_extrema, continuous_minima, continuous_maxima);
+                to_continuous(s->observation_space.continuous_extrema, continuous_minima,
+                              continuous_maxima);
 
                 if (num_entities > 1 && global_sensor_) {
                     done = true;
@@ -187,8 +182,7 @@ void OpenAIObservations::create_observation_space(size_t num_entities,
             }
         }
 
-        observation_space =
-            create_space(discrete_count, continuous_minima, continuous_maxima);
+        observation_space = create_space(discrete_count, continuous_minima, continuous_maxima);
 
         observation = create_obs(discrete_count, continuous_maxima);
 
@@ -196,18 +190,18 @@ void OpenAIObservations::create_observation_space(size_t num_entities,
         py::list observation_spaces;
         py::list obs;
 
-        for (auto &v : ext_sensor_vec_) {
-            for (auto &s : v) {
+        for (auto& v : ext_sensor_vec_) {
+            for (auto& s : v) {
                 py::list discrete_count;
                 py::list continuous_minima;
                 py::list continuous_maxima;
 
                 s->set_observation_space();
                 to_discrete(s->observation_space.discrete_count, discrete_count);
-                to_continuous(s->observation_space.continuous_extrema, continuous_minima, continuous_maxima);
+                to_continuous(s->observation_space.continuous_extrema, continuous_minima,
+                              continuous_maxima);
 
-                auto space = create_space(discrete_count, continuous_minima,
-                                          continuous_maxima);
+                auto space = create_space(discrete_count, continuous_minima, continuous_maxima);
                 observation_spaces.append(space);
                 obs.append(create_obs(discrete_count, continuous_maxima));
             }
@@ -217,17 +211,18 @@ void OpenAIObservations::create_observation_space(size_t num_entities,
         observation_space = tuple_space(observation_spaces);
         observation = obs;
 
-        py::list observation_space_list =
-                observation_space.attr("spaces").cast<py::list>();
+        py::list observation_space_list = observation_space.attr("spaces").cast<py::list>();
     }
 }
 
-void OpenAIObservations::add_sensors(const std::unordered_map<std::string, SensorPtr> &sensors) {
+void OpenAIObservations::add_sensors(const std::unordered_map<std::string, SensorPtr>& sensors) {
     std::vector<std::shared_ptr<sensor::ScrimmageOpenAISensor>> vec;
-    auto cast = [&](auto &p) {return std::dynamic_pointer_cast<sensor::ScrimmageOpenAISensor>(p);};
-    auto good_ptr = [&](auto &p) {return cast(p) != nullptr;};
+    auto cast = [&](auto& p) {
+        return std::dynamic_pointer_cast<sensor::ScrimmageOpenAISensor>(p);
+    };
+    auto good_ptr = [&](auto& p) { return cast(p) != nullptr; };
     br::transform(sensors | ba::map_values | ba::filtered(good_ptr), std::back_inserter(vec), cast);
     ext_sensor_vec_.push_back(vec);
 }
-} // namespace autonomy
-} // namespace scrimmage
+}  // namespace autonomy
+}  // namespace scrimmage
diff --git a/src/plugins/autonomy/ScrimmageOpenAIAutonomy/OpenAIUtils.cpp b/src/plugins/autonomy/ScrimmageOpenAIAutonomy/OpenAIUtils.cpp
index d859c73e0a..989531e158 100644
--- a/src/plugins/autonomy/ScrimmageOpenAIAutonomy/OpenAIUtils.cpp
+++ b/src/plugins/autonomy/ScrimmageOpenAIAutonomy/OpenAIUtils.cpp
@@ -30,36 +30,31 @@
  *
  */
 
-#include <scrimmage/plugins/autonomy/ScrimmageOpenAIAutonomy/OpenAIUtils.h>
-
 #include <pybind11/numpy.h>
+#include <scrimmage/plugins/autonomy/ScrimmageOpenAIAutonomy/OpenAIUtils.h>
 
 namespace py = pybind11;
 
 namespace scrimmage {
 namespace autonomy {
 
-void to_continuous(std::vector<std::pair<double, double>> &p,
-                   pybind11::list &minima,
-                   pybind11::list &maxima) {
-    for (auto &value : p) {
+void to_continuous(std::vector<std::pair<double, double>>& p, pybind11::list& minima,
+                   pybind11::list& maxima) {
+    for (auto& value : p) {
         py::list min_max;
         minima.append(value.first);
         maxima.append(value.second);
     }
 }
 
-void to_discrete(std::vector<int> &p, py::list &maxima) {
-    for (auto &value : p) {
+void to_discrete(std::vector<int>& p, py::list& maxima) {
+    for (auto& value : p) {
         maxima.append(value);
     }
 }
 
-pybind11::object create_space(
-        pybind11::list discrete_count,
-        pybind11::list continuous_minima,
-        pybind11::list continuous_maxima) {
-
+pybind11::object create_space(pybind11::list discrete_count, pybind11::list continuous_minima,
+                              pybind11::list continuous_maxima) {
     py::module np = py::module::import("numpy");
     py::object np_array = np.attr("array");
     py::object np_float32 = np.attr("float32");
@@ -69,15 +64,12 @@ pybind11::object create_space(
     py::object gym_box_space = get_gym_space("Box");
     py::object gym_tuple_space = get_gym_space("Tuple");
 
-    py::object discrete_space = py::len(discrete_count) == 1 ?
-        gym_discrete_space(discrete_count[0]) :
-        gym_multidiscrete_space(discrete_count);
+    py::object discrete_space = py::len(discrete_count) == 1
+                                    ? gym_discrete_space(discrete_count[0])
+                                    : gym_multidiscrete_space(discrete_count);
 
     py::object continuous_space = gym_box_space(
-        np_array(continuous_minima),
-        np_array(continuous_maxima),
-        py::none(),
-        np_float32);
+        np_array(continuous_minima), np_array(continuous_maxima), py::none(), np_float32);
 
     int len_discrete = py::len(discrete_count);
     int len_continuous = py::len(continuous_minima);
@@ -91,13 +83,12 @@ pybind11::object create_space(
     } else if (len_continuous != 0) {
         return continuous_space;
     } else {
-        throw std::runtime_error(
-            "discrete and continuous space do not have maxima set");
+        throw std::runtime_error("discrete and continuous space do not have maxima set");
     }
 }
 
-pybind11::object get_gym_space(const std::string &type) {
+pybind11::object get_gym_space(const std::string& type) {
     return pybind11::module::import("gym.spaces").attr(type.c_str());
 }
-} // namespace autonomy
-} // namespace scrimmage
+}  // namespace autonomy
+}  // namespace scrimmage
diff --git a/src/plugins/autonomy/ScrimmageOpenAIAutonomy/ScrimmageOpenAIAutonomy.cpp b/src/plugins/autonomy/ScrimmageOpenAIAutonomy/ScrimmageOpenAIAutonomy.cpp
index 71966238f3..c245324c57 100644
--- a/src/plugins/autonomy/ScrimmageOpenAIAutonomy/ScrimmageOpenAIAutonomy.cpp
+++ b/src/plugins/autonomy/ScrimmageOpenAIAutonomy/ScrimmageOpenAIAutonomy.cpp
@@ -32,17 +32,16 @@
 
 #include <pybind11/pybind11.h>
 #include <pybind11/stl.h>
-
 #include <scrimmage/entity/Entity.h>
+#include <scrimmage/parse/MissionParse.h>
 #include <scrimmage/parse/ParseUtils.h>
-#include <scrimmage/plugins/autonomy/ScrimmageOpenAIAutonomy/ScrimmageOpenAIAutonomy.h>
+#include <scrimmage/plugin_manager/RegisterPlugin.h>
 #include <scrimmage/plugins/autonomy/ScrimmageOpenAIAutonomy/ActorFunc.h>
+#include <scrimmage/plugins/autonomy/ScrimmageOpenAIAutonomy/ScrimmageOpenAIAutonomy.h>
 #include <scrimmage/plugins/sensor/ScrimmageOpenAISensor/ScrimmageOpenAISensor.h>
-#include <scrimmage/plugin_manager/RegisterPlugin.h>
 #include <scrimmage/pubsub/Message.h>
 #include <scrimmage/pubsub/Publisher.h>
 #include <scrimmage/sensor/Sensor.h>
-#include <scrimmage/parse/MissionParse.h>
 
 #include <limits>
 
@@ -51,8 +50,8 @@
 using grpc::Channel;
 using grpc::ClientContext;
 using grpc::Status;
-#include <chrono> //NOLINT
-#include <thread> //NOLINT
+#include <chrono>  //NOLINT
+#include <thread>  //NOLINT
 #endif
 
 namespace sp = scrimmage_proto;
@@ -61,19 +60,21 @@ namespace py = pybind11;
 using std::cout;
 using std::endl;
 
-REGISTER_PLUGIN(scrimmage::Autonomy, scrimmage::autonomy::ScrimmageOpenAIAutonomy, ScrimmageOpenAIAutonomy_plugin)
+REGISTER_PLUGIN(scrimmage::Autonomy, scrimmage::autonomy::ScrimmageOpenAIAutonomy,
+                ScrimmageOpenAIAutonomy_plugin)
 
 namespace scrimmage {
 namespace autonomy {
 
-ScrimmageOpenAIAutonomy::ScrimmageOpenAIAutonomy() :
-        reward_range(-std::numeric_limits<double>::infinity(),
-                     std::numeric_limits<double>::infinity()),
-        tuple_space_(get_gym_space("Tuple")),
-        box_space_(get_gym_space("Box")),
-        nonlearning_mode_(false) {}
+ScrimmageOpenAIAutonomy::ScrimmageOpenAIAutonomy()
+    : reward_range(-std::numeric_limits<double>::infinity(),
+                   std::numeric_limits<double>::infinity()),
+      tuple_space_(get_gym_space("Tuple")),
+      box_space_(get_gym_space("Box")),
+      nonlearning_mode_(false) {
+}
 
-void ScrimmageOpenAIAutonomy::init(std::map<std::string, std::string> &params) {
+void ScrimmageOpenAIAutonomy::init(std::map<std::string, std::string>& params) {
     init_helper(params);
     nonlearning_mode_ = get("nonlearning_mode_openai_plugin", params, true);
     self_id_ = parent_->id().id();
@@ -93,9 +94,8 @@ void ScrimmageOpenAIAutonomy::init(std::map<std::string, std::string> &params) {
         const std::string port_str = std::to_string(port + parent_->id().id());
         const std::string address = grpc_address_ + ":" + port_str;
 
-        python_cmd_ = std::string("openai_grpc_link.py") + " --port " + port_str
-                + " --actor " + module + ":" + actor_func_str
-                + " --ip " + grpc_address_;
+        python_cmd_ = std::string("openai_grpc_link.py") + " --port " + port_str + " --actor "
+                      + module + ":" + actor_func_str + " --ip " + grpc_address_;
         auto channel = grpc::CreateChannel(address, grpc::InsecureChannelCredentials());
         openai_stub_ = sp::OpenAI::NewStub(channel);
     }
@@ -111,26 +111,24 @@ bool ScrimmageOpenAIAutonomy::step_autonomy(double t, double /*dt*/) {
             // need contacts size to be set before calling init
             auto p = std::dynamic_pointer_cast<ScrimmageOpenAIAutonomy>(shared_from_this());
             std::tie(actions_, observations_, actor_func_) =
-                    init_actor_func({p}, params_, CombineActors::NO,
-                                    UseGlobalSensor::NO,
-                                    grpc_mode_);
+                init_actor_func({p}, params_, CombineActors::NO, UseGlobalSensor::NO, grpc_mode_);
 
 #if ENABLE_GRPC
             if (grpc_mode_) {
                 int attempts = 0;
                 bool done = false;
                 while (attempts++ < 10 && !done) {
-                    std::cout << "Connecting to gRPC Server attempt: " << attempts << "/10" << std::endl;
+                    std::cout << "Connecting to gRPC Server attempt: " << attempts << "/10"
+                              << std::endl;
                     done = send_env();
                 }
                 // Return an exception saying we could not connect to grpc
                 if (!done) {
                     const std::string err_msg =
-                            std::string("Didn't connect to the grpc server. Make sure")
-                            + " the grpc client is running and your python"
-                            + " bindings are up to date."
-                            + "You can start a grpc server with the followning command: "
-                            + python_cmd_;
+                        std::string("Didn't connect to the grpc server. Make sure")
+                        + " the grpc client is running and your python"
+                        + " bindings are up to date."
+                        + "You can start a grpc server with the followning command: " + python_cmd_;
                     throw std::runtime_error(err_msg);
                 }
             }
@@ -158,8 +156,7 @@ bool ScrimmageOpenAIAutonomy::step_autonomy(double t, double /*dt*/) {
 
         temp_action = actor_func_(observations_.observation);
 
-        actions_.distribute_action(temp_action,
-                                   observations_.get_combine_actors());
+        actions_.distribute_action(temp_action, observations_.get_combine_actors());
 
         if (first_step_) {
             // there is no reward on the first step
@@ -178,17 +175,16 @@ bool ScrimmageOpenAIAutonomy::step_autonomy(double t, double /*dt*/) {
 }
 
 #if ENABLE_GRPC
-boost::optional<sp::Action> ScrimmageOpenAIAutonomy::get_action(sp::Obs &observation) {
+boost::optional<sp::Action> ScrimmageOpenAIAutonomy::get_action(sp::Obs& observation) {
     if (!openai_stub_) return boost::none;
 
     sp::Action action;
     grpc::ClientContext context;
-    auto deadline = std::chrono::system_clock::now() +
-            std::chrono::milliseconds(500);
+    auto deadline = std::chrono::system_clock::now() + std::chrono::milliseconds(500);
     context.set_deadline(deadline);
     grpc::Status status = openai_stub_->GetAction(&context, observation, &action);
-    if (!(status.error_code() == grpc::StatusCode::OK ||
-          status.error_code() == grpc::StatusCode::UNKNOWN)) {
+    if (!(status.error_code() == grpc::StatusCode::OK
+          || status.error_code() == grpc::StatusCode::UNKNOWN)) {
         return boost::none;
     } else if (action.done()) {
         return boost::none;
@@ -203,7 +199,7 @@ bool ScrimmageOpenAIAutonomy::send_env() {
     auto discrete_space = env.mutable_action_spaces()->mutable_discrete();
     *(discrete_space) = {action_space.discrete_count.begin(), action_space.discrete_count.end()};
     for (std::pair<double, double> cont_extrema : action_space.continuous_extrema) {
-        sp::ContExtrema * proto_extrema = env.mutable_action_spaces()->add_continuous();
+        sp::ContExtrema* proto_extrema = env.mutable_action_spaces()->add_continuous();
         proto_extrema->set_min(cont_extrema.first);
         proto_extrema->set_max(cont_extrema.second);
     }
@@ -219,7 +215,7 @@ bool ScrimmageOpenAIAutonomy::send_env() {
             }
             // Add continuous/ observation spaces to the protobuf environment
             for (std::pair<double, double> cont_extrema : obs_space.continuous_extrema) {
-                sp::ContExtrema * proto_extrema = env.mutable_observation_spaces()->add_continuous();
+                sp::ContExtrema* proto_extrema = env.mutable_observation_spaces()->add_continuous();
                 proto_extrema->set_min(cont_extrema.first);
                 proto_extrema->set_max(cont_extrema.second);
             }
@@ -227,7 +223,7 @@ bool ScrimmageOpenAIAutonomy::send_env() {
     }
     // Send over params through GRPC
     std::vector<std::string> necessary_params = {"logdir", "hyperparams_file"};
-    auto &proto_map = *env.mutable_params();
+    auto& proto_map = *env.mutable_params();
     for (std::string param_key : necessary_params) {
         auto search = params_.find(param_key);
         if (search != params_.end()) {
@@ -243,8 +239,8 @@ bool ScrimmageOpenAIAutonomy::send_env() {
     context.set_deadline(deadline);
     grpc::Status status = openai_stub_->SendEnvironment(&context, env, &reply);
     if (!status.ok()) {
-        std::cout << "Can't Connect: message " << status.error_code()
-                  << ", " << status.error_message() << std::endl;
+        std::cout << "Can't Connect: message " << status.error_code() << ", "
+                  << status.error_message() << std::endl;
     }
     return status.ok();
 }
@@ -273,7 +269,7 @@ sp::Obs ScrimmageOpenAIAutonomy::obs_to_proto() {
     return proto_obs;
 }
 
-pybind11::object ScrimmageOpenAIAutonomy::convert_proto_action(const sp::Action &proto_act) {
+pybind11::object ScrimmageOpenAIAutonomy::convert_proto_action(const sp::Action& proto_act) {
     pybind11::list py_act;
     std::vector<int> disc(proto_act.discrete_size(), 0);
     std::vector<double> cont(proto_act.continuous_size(), 0);
@@ -316,5 +312,5 @@ std::tuple<bool, double, pybind11::dict> ScrimmageOpenAIAutonomy::calc_reward()
     return std::make_tuple(false, 0.0, pybind11::dict());
 }
 
-} // namespace autonomy
-} // namespace scrimmage
+}  // namespace autonomy
+}  // namespace scrimmage
diff --git a/src/plugins/autonomy/ShapeDraw/ShapeDraw.cpp b/src/plugins/autonomy/ShapeDraw/ShapeDraw.cpp
index 6c19d16665..5a7899200c 100644
--- a/src/plugins/autonomy/ShapeDraw/ShapeDraw.cpp
+++ b/src/plugins/autonomy/ShapeDraw/ShapeDraw.cpp
@@ -30,16 +30,15 @@
  *
  */
 
-#include <scrimmage/plugins/autonomy/ShapeDraw/ShapeDraw.h>
-
-#include <scrimmage/plugin_manager/RegisterPlugin.h>
 #include <scrimmage/entity/Entity.h>
-#include <scrimmage/math/State.h>
 #include <scrimmage/math/Angles.h>
 #include <scrimmage/math/Quaternion.h>
+#include <scrimmage/math/State.h>
 #include <scrimmage/parse/ParseUtils.h>
-#include <scrimmage/proto/Shape.pb.h>
+#include <scrimmage/plugin_manager/RegisterPlugin.h>
+#include <scrimmage/plugins/autonomy/ShapeDraw/ShapeDraw.h>
 #include <scrimmage/proto/ProtoConversions.h>
+#include <scrimmage/proto/Shape.pb.h>
 
 #include <iostream>
 #include <limits>
@@ -49,9 +48,7 @@ using std::endl;
 
 namespace sc = scrimmage;
 
-REGISTER_PLUGIN(scrimmage::Autonomy,
-                scrimmage::autonomy::ShapeDraw,
-                ShapeDraw_plugin)
+REGISTER_PLUGIN(scrimmage::Autonomy, scrimmage::autonomy::ShapeDraw, ShapeDraw_plugin)
 
 namespace scrimmage {
 namespace autonomy {
@@ -59,12 +56,12 @@ namespace autonomy {
 ShapeDraw::ShapeDraw() : follow_id_(-1), init_(true) {
 }
 
-void ShapeDraw::init(std::map<std::string, std::string> &params) {
+void ShapeDraw::init(std::map<std::string, std::string>& params) {
     double initial_speed = sc::get<double>("initial_speed", params, 21);
 
-    desired_state_->vel() = Eigen::Vector3d::UnitX()*initial_speed;
+    desired_state_->vel() = Eigen::Vector3d::UnitX() * initial_speed;
     desired_state_->quat().set(0, 0, state_->quat().yaw());
-    desired_state_->pos() = Eigen::Vector3d::UnitZ()*state_->pos()(2);
+    desired_state_->pos() = Eigen::Vector3d::UnitZ() * state_->pos()(2);
 
     ellipse_shape_ = std::make_shared<scrimmage_proto::Shape>();
     cuboid_shape_ = std::make_shared<scrimmage_proto::Shape>();
@@ -89,8 +86,7 @@ void ShapeDraw::draw_ellipse(double t, double dt) {
     ellipse_shape_->mutable_ellipse()->set_x_radius(10);
     ellipse_shape_->mutable_ellipse()->set_y_radius(5);
 
-    sc::Quaternion quat(sc::Angles::deg2rad(0.0),
-                        sc::Angles::deg2rad(0.0),
+    sc::Quaternion quat(sc::Angles::deg2rad(0.0), sc::Angles::deg2rad(0.0),
                         sc::Angles::deg2rad(45.0));
     sc::set(ellipse_shape_->mutable_ellipse()->mutable_quat(), quat);
 
@@ -107,8 +103,7 @@ void ShapeDraw::draw_cuboid(double t, double dt) {
     cuboid_shape_->mutable_cuboid()->set_y_length(5);
     cuboid_shape_->mutable_cuboid()->set_z_length(5);
 
-    sc::Quaternion quat(sc::Angles::deg2rad(0.0),
-                        sc::Angles::deg2rad(45.0),
+    sc::Quaternion quat(sc::Angles::deg2rad(0.0), sc::Angles::deg2rad(45.0),
                         sc::Angles::deg2rad(45.0));
     sc::set(cuboid_shape_->mutable_cuboid()->mutable_quat(), quat);
 
@@ -123,8 +118,7 @@ void ShapeDraw::draw_mesh(double t, double dt) {
     // corresponds to zephyr-red.xml
     mesh_shape_->mutable_mesh()->set_name("zephyr-red");
 
-    sc::Quaternion quat(sc::Angles::deg2rad(0.0),
-                        sc::Angles::deg2rad(45.0),
+    sc::Quaternion quat(sc::Angles::deg2rad(0.0), sc::Angles::deg2rad(45.0),
                         sc::Angles::deg2rad(90.0));
     sc::set(mesh_shape_->mutable_mesh()->mutable_quat(), quat);
     mesh_shape_->mutable_mesh()->set_scale(10.0);
@@ -135,5 +129,5 @@ void ShapeDraw::draw_mesh(double t, double dt) {
     draw_shape(mesh_shape_);
 }
 
-} // namespace autonomy
-} // namespace scrimmage
+}  // namespace autonomy
+}  // namespace scrimmage
diff --git a/src/plugins/autonomy/Straight/Straight.cpp b/src/plugins/autonomy/Straight/Straight.cpp
index 8d714d3be7..dbeef9b31b 100644
--- a/src/plugins/autonomy/Straight/Straight.cpp
+++ b/src/plugins/autonomy/Straight/Straight.cpp
@@ -30,26 +30,27 @@
  *
  */
 
-#include <scrimmage/common/Utilities.h>
 #include <scrimmage/common/Shape.h>
 #include <scrimmage/common/Time.h>
+#include <scrimmage/common/Utilities.h>
 #include <scrimmage/entity/Entity.h>
+#include <scrimmage/math/Angles.h>
 #include <scrimmage/math/State.h>
 #include <scrimmage/math/StateWithCovariance.h>
-#include <scrimmage/math/Angles.h>
+#include <scrimmage/msgs/Event.pb.h>
 #include <scrimmage/parse/ParseUtils.h>
 #include <scrimmage/plugin_manager/RegisterPlugin.h>
-#include <scrimmage/plugins/interaction/Boundary/BoundaryBase.h>
 #include <scrimmage/plugins/interaction/Boundary/Boundary.h>
+#include <scrimmage/plugins/interaction/Boundary/BoundaryBase.h>
 #include <scrimmage/proto/State.pb.h>
-#include <scrimmage/msgs/Event.pb.h>
 #include <scrimmage/pubsub/Message.h>
-#include <scrimmage/pubsub/Subscriber.h>
 #include <scrimmage/pubsub/Publisher.h>
+#include <scrimmage/pubsub/Subscriber.h>
 #include <scrimmage/sensor/Sensor.h>
 
 #if ENABLE_OPENCV == 1
 #include <scrimmage/plugins/sensor/ContactBlobCamera/ContactBlobCameraType.h>
+
 #include <opencv2/core/core.hpp>
 #include <opencv2/highgui/highgui.hpp>
 #include <opencv2/imgproc/imgproc.hpp>
@@ -72,21 +73,19 @@ namespace sci = scrimmage::interaction;
 #undef BOOST_NO_CXX11_SCOPED_ENUMS
 namespace fs = boost::filesystem;
 
-REGISTER_PLUGIN(scrimmage::Autonomy,
-                scrimmage::autonomy::Straight,
-                Straight_plugin)
+REGISTER_PLUGIN(scrimmage::Autonomy, scrimmage::autonomy::Straight, Straight_plugin)
 
 namespace scrimmage {
 namespace autonomy {
 
-void Straight::init(std::map<std::string, std::string> &params) {
+void Straight::init(std::map<std::string, std::string>& params) {
     speed_ = scrimmage::get("speed", params, 0.0);
     show_camera_images_ = scrimmage::get<bool>("show_camera_images", params, false);
     save_camera_images_ = scrimmage::get<bool>("save_camera_images", params, false);
     show_text_label_ = scrimmage::get<bool>("show_text_label", params, false);
 
     // Project goal in front...
-    Eigen::Vector3d rel_pos = Eigen::Vector3d::UnitX()*1e6;
+    Eigen::Vector3d rel_pos = Eigen::Vector3d::UnitX() * 1e6;
     Eigen::Vector3d unit_vector = rel_pos.normalized();
     unit_vector = state_->quat().rotate(unit_vector);
     goal_ = state_->pos() + unit_vector * rel_pos.norm();
@@ -96,9 +95,9 @@ void Straight::init(std::map<std::string, std::string> &params) {
     // desired_z_ = state_->pos()(2);
 
     // Register the desired_z parameter with the parameter server
-    auto param_cb = [&](const double &desired_z) {
-        std::cout << "desired_z param changed at: " << time_->t()
-        << ", with value: " << desired_z << endl;
+    auto param_cb = [&](const double& desired_z) {
+        std::cout << "desired_z param changed at: " << time_->t() << ", with value: " << desired_z
+                  << endl;
     };
     register_param<double>("desired_z", goal_(2), param_cb);
 
@@ -130,8 +129,8 @@ void Straight::init(std::map<std::string, std::string> &params) {
     if (show_text_label_) {
         // Draw a text label (white text) 30 meters in front of vehicle:
         Eigen::Vector3d in_front = state_->pos() + unit_vector * 30;
-        text_shape_ = sc::shape::make_text("Hello, SCRIMMAGE!", in_front,
-                                           Eigen::Vector3d(255, 255, 255));
+        text_shape_ =
+            sc::shape::make_text("Hello, SCRIMMAGE!", in_front, Eigen::Vector3d(255, 255, 255));
 
         // Draw the shape in the 3D viewer
         draw_shape(text_shape_);
@@ -139,31 +138,33 @@ void Straight::init(std::map<std::string, std::string> &params) {
 
     enable_boundary_control_ = get<bool>("enable_boundary_control", params, false);
 
-    auto bd_cb = [&](auto &msg) {boundary_ = sci::Boundary::make_boundary(msg->data);};
+    auto bd_cb = [&](auto& msg) { boundary_ = sci::Boundary::make_boundary(msg->data); };
     subscribe<sp::Shape>("GlobalNetwork", "Boundary", bd_cb);
 
-    auto state_cb = [&](auto &msg) {
+    auto state_cb = [&](auto& msg) {
         noisy_state_set_ = true;
         noisy_state_ = msg->data;
     };
     subscribe<StateWithCovariance>("LocalNetwork", "StateWithCovariance", state_cb);
 
-    auto cnt_cb = [&](scrimmage::MessagePtr<ContactMap> &msg) {
-        noisy_contacts_ = msg->data; // Save map of noisy contacts
+    auto cnt_cb = [&](scrimmage::MessagePtr<ContactMap>& msg) {
+        noisy_contacts_ = msg->data;  // Save map of noisy contacts
     };
     subscribe<ContactMap>("LocalNetwork", "ContactsWithCovariances", cnt_cb);
 
 #if (ENABLE_OPENCV == 1 && ENABLE_AIRSIM == 1)
-    auto airsim_cb = [&](auto &msg) {
+    auto airsim_cb = [&](auto& msg) {
         for (sc::sensor::AirSimImageType a : msg->data) {
             if (show_camera_images_) {
                 // Get Camera Name
-                std::string window_name = a.vehicle_name + "_" + a.camera_config.cam_name + "_" + a.camera_config.img_type_name;
+                std::string window_name = a.vehicle_name + "_" + a.camera_config.cam_name + "_"
+                                          + a.camera_config.img_type_name;
                 // for depth images CV imshow expects grayscale image values to be between 0 and 1.
-                if (a.camera_config.img_type_name == "DepthPerspective" || a.camera_config.img_type_name == "DepthPlanner") {
+                if (a.camera_config.img_type_name == "DepthPerspective"
+                    || a.camera_config.img_type_name == "DepthPlanner") {
                     // Worked before building with ROS
                     cv::Mat tempImage;
-                    a.img.convertTo(tempImage, CV_32FC1, 1.f/255);
+                    a.img.convertTo(tempImage, CV_32FC1, 1.f / 255);
                     // cv::normalize(a.img, tempImage, 0, 1, cv::NORM_MINMAX);
                     // cout << tempImage << endl;
                     cv::imshow(window_name, tempImage);
@@ -171,10 +172,12 @@ void Straight::init(std::map<std::string, std::string> &params) {
                     // other image types are int 0-255.
                     if (a.img.channels() == 4) {
                         cout << "image channels: " << a.img.channels() << endl;
-                        cout << "Warning: Old AirSim Linux Asset Environments have 4 channels. Color images will not display correctly." << endl;
+                        cout << "Warning: Old AirSim Linux Asset Environments have 4 channels. "
+                                "Color images will not display correctly."
+                             << endl;
                         cout << "Warning: Use Asset Environment versions Linux-v1.3.1+." << endl;
                         cv::Mat tempImage;
-                        cv::cvtColor(a.img , tempImage, CV_RGBA2RGB);
+                        cv::cvtColor(a.img, tempImage, CV_RGBA2RGB);
                         cv::imshow(window_name, tempImage);
                     } else {
                         cv::imshow(window_name, a.img);
@@ -188,10 +191,9 @@ void Straight::init(std::map<std::string, std::string> &params) {
 #endif
 
 #if ENABLE_OPENCV == 1
-    auto blob_cb = [&](auto &msg) {
+    auto blob_cb = [&](auto& msg) {
         if (save_camera_images_) {
-            std::string img_name = "./imgs/camera_" +
-                std::to_string(frame_number_++) + ".png";
+            std::string img_name = "./imgs/camera_" + std::to_string(frame_number_++) + ".png";
             cv::imwrite(img_name, msg->data.frame);
         }
 
@@ -208,9 +210,11 @@ void Straight::init(std::map<std::string, std::string> &params) {
         pub_gen_ents_ = advertise("GlobalNetwork", "GenerateEntity");
     }
 
-    desired_alt_idx_ = vars_.declare(VariableIO::Type::desired_altitude, VariableIO::Direction::Out);
+    desired_alt_idx_ =
+        vars_.declare(VariableIO::Type::desired_altitude, VariableIO::Direction::Out);
     desired_speed_idx_ = vars_.declare(VariableIO::Type::desired_speed, VariableIO::Direction::Out);
-    desired_heading_idx_ = vars_.declare(VariableIO::Type::desired_heading, VariableIO::Direction::Out);
+    desired_heading_idx_ =
+        vars_.declare(VariableIO::Type::desired_heading, VariableIO::Direction::Out);
 }
 
 bool Straight::step_autonomy(double t, double dt) {
@@ -224,13 +228,13 @@ bool Straight::step_autonomy(double t, double dt) {
             // Rotate the heading of the new entity by 90 degrees and offset
             // the initial position to the left of our current position to
             // avoid collisions.
-            Eigen::AngleAxisd rot_90_z(M_PI/2.0, Eigen::Vector3d::UnitZ());
+            Eigen::AngleAxisd rot_90_z(M_PI / 2.0, Eigen::Vector3d::UnitZ());
             s.pos() = s.pos() + s.quat() * rot_90_z * (Eigen::Vector3d::UnitX() * 10);
             s.quat() = rot_90_z * s.quat();
 
             // Create the GenerateEntity message
             auto msg = std::make_shared<Message<scrimmage_msgs::GenerateEntity>>();
-            sc::set(msg->data.mutable_state(), s); // Copy the new state
+            sc::set(msg->data.mutable_state(), s);  // Copy the new state
 
             // The entity_tag must match the "tag" XML attribute of the entity
             // to be generated in the mission file.
@@ -246,7 +250,7 @@ bool Straight::step_autonomy(double t, double dt) {
             kv_visual->set_key("visual_model");
             kv_visual->set_value("sphere");
 
-            pub_gen_ents_->publish(msg); // Publish the GenerateEntity message
+            pub_gen_ents_->publish(msg);  // Publish the GenerateEntity message
         }
     }
 
@@ -274,7 +278,7 @@ bool Straight::step_autonomy(double t, double dt) {
         if (!boundary_->contains(noisy_state_.pos())) {
             // Project goal through center of boundary
             Eigen::Vector3d center = boundary_->center();
-            center(2) = noisy_state_.pos()(2); // maintain altitude
+            center(2) = noisy_state_.pos()(2);  // maintain altitude
             Eigen::Vector3d diff = center - noisy_state_.pos();
             goal_ = noisy_state_.pos() + diff.normalized() * 1e6;
         }
@@ -294,5 +298,5 @@ bool Straight::step_autonomy(double t, double dt) {
     noisy_state_set_ = false;
     return true;
 }
-} // namespace autonomy
-} // namespace scrimmage
+}  // namespace autonomy
+}  // namespace scrimmage
diff --git a/src/plugins/autonomy/TakeFlag/TakeFlag.cpp b/src/plugins/autonomy/TakeFlag/TakeFlag.cpp
index c32b3d2049..204efae6ef 100644
--- a/src/plugins/autonomy/TakeFlag/TakeFlag.cpp
+++ b/src/plugins/autonomy/TakeFlag/TakeFlag.cpp
@@ -30,37 +30,30 @@
  *
  */
 
-#include <scrimmage/plugins/autonomy/TakeFlag/TakeFlag.h>
-
-#include <scrimmage/plugin_manager/RegisterPlugin.h>
+#include <scrimmage/common/Waypoint.h>
 #include <scrimmage/entity/Entity.h>
 #include <scrimmage/math/State.h>
+#include <scrimmage/msgs/Capture.pb.h>
 #include <scrimmage/parse/ParseUtils.h>
+#include <scrimmage/plugin_manager/RegisterPlugin.h>
+#include <scrimmage/plugins/autonomy/TakeFlag/TakeFlag.h>
+#include <scrimmage/plugins/autonomy/WaypointGenerator/WaypointList.h>
+#include <scrimmage/plugins/interaction/Boundary/Boundary.h>
+#include <scrimmage/plugins/interaction/Boundary/BoundaryBase.h>
 #include <scrimmage/pubsub/Publisher.h>
 #include <scrimmage/pubsub/Subscriber.h>
 
-#include <scrimmage/plugins/interaction/Boundary/BoundaryBase.h>
-#include <scrimmage/plugins/interaction/Boundary/Boundary.h>
-
-#include <scrimmage/common/Waypoint.h>
-#include <scrimmage/plugins/autonomy/WaypointGenerator/WaypointList.h>
-
-#include <scrimmage/msgs/Capture.pb.h>
-
+#include <GeographicLib/LocalCartesian.hpp>
 #include <iostream>
 #include <limits>
 
-#include <GeographicLib/LocalCartesian.hpp>
-
 using std::cout;
 using std::endl;
 
 namespace sm = scrimmage_msgs;
 namespace sci = scrimmage::interaction;
 
-REGISTER_PLUGIN(scrimmage::Autonomy,
-                scrimmage::autonomy::TakeFlag,
-                TakeFlag_plugin)
+REGISTER_PLUGIN(scrimmage::Autonomy, scrimmage::autonomy::TakeFlag, TakeFlag_plugin)
 
 namespace scrimmage {
 namespace autonomy {
@@ -68,19 +61,19 @@ namespace autonomy {
 TakeFlag::TakeFlag() {
 }
 
-void TakeFlag::init(std::map<std::string, std::string> &params) {
+void TakeFlag::init(std::map<std::string, std::string>& params) {
     flag_boundary_id_ = get<int>("flag_boundary_id", params, 1);
     capture_boundary_id_ = get<int>("capture_boundary_id", params, 1);
 
-    auto callback = [&] (scrimmage::MessagePtr<sp::Shape> msg) {
+    auto callback = [&](scrimmage::MessagePtr<sp::Shape> msg) {
         std::shared_ptr<sci::BoundaryBase> boundary = sci::Boundary::make_boundary(msg->data);
         boundaries_[msg->data.id().id()] = std::make_pair(msg->data, boundary);
     };
     subscribe<sp::Shape>("GlobalNetwork", "Boundary", callback);
 
-    auto flag_taken_cb = [&] (scrimmage::MessagePtr<sm::FlagTaken> msg) {
-        if (msg->data.entity_id() == parent_->id().id() &&
-            msg->data.flag_boundary_id() == flag_boundary_id_) {
+    auto flag_taken_cb = [&](scrimmage::MessagePtr<sm::FlagTaken> msg) {
+        if (msg->data.entity_id() == parent_->id().id()
+            && msg->data.flag_boundary_id() == flag_boundary_id_) {
             has_flag_ = true;
         }
     };
@@ -92,7 +85,6 @@ void TakeFlag::init(std::map<std::string, std::string> &params) {
 }
 
 bool TakeFlag::step_autonomy(double t, double dt) {
-
     Eigen::Vector3d desired_point = state_->pos();
     if (!has_flag_) {
         // If we don't have the flag yet, head towards it!
@@ -119,5 +111,5 @@ bool TakeFlag::step_autonomy(double t, double dt) {
     return true;
 }
 
-} // namespace autonomy
-} // namespace scrimmage
+}  // namespace autonomy
+}  // namespace scrimmage
diff --git a/src/plugins/autonomy/TrailMS/TrailMS.cpp b/src/plugins/autonomy/TrailMS/TrailMS.cpp
index 788047cd60..74827e8e33 100644
--- a/src/plugins/autonomy/TrailMS/TrailMS.cpp
+++ b/src/plugins/autonomy/TrailMS/TrailMS.cpp
@@ -30,16 +30,15 @@
  *
  */
 
-#include <scrimmage/plugins/autonomy/TrailMS/TrailMS.h>
-
-#include <scrimmage/plugin_manager/RegisterPlugin.h>
+#include <scrimmage/common/Shape.h>
 #include <scrimmage/entity/Entity.h>
+#include <scrimmage/math/Angles.h>
 #include <scrimmage/math/State.h>
-#include <scrimmage/common/Shape.h>
 #include <scrimmage/parse/ParseUtils.h>
-#include <scrimmage/proto/Shape.pb.h>
+#include <scrimmage/plugin_manager/RegisterPlugin.h>
+#include <scrimmage/plugins/autonomy/TrailMS/TrailMS.h>
 #include <scrimmage/proto/ProtoConversions.h>
-#include <scrimmage/math/Angles.h>
+#include <scrimmage/proto/Shape.pb.h>
 
 #include <iostream>
 #include <limits>
@@ -50,20 +49,20 @@ using std::endl;
 namespace sc = scrimmage;
 namespace sp = scrimmage_proto;
 
-REGISTER_PLUGIN(scrimmage::Autonomy,
-                scrimmage::autonomy::TrailMS,
-                TrailMS_plugin)
+REGISTER_PLUGIN(scrimmage::Autonomy, scrimmage::autonomy::TrailMS, TrailMS_plugin)
 
 namespace scrimmage {
 namespace autonomy {
 
-void TrailMS::init(std::map<std::string, std::string> &params) {
+void TrailMS::init(std::map<std::string, std::string>& params) {
     trail_id_ = sc::get<int>("trail_id", params, trail_id_);
     trail_range_ = sc::get<double>("trail_range", params, trail_range_);
     show_track_point_ = sc::get<bool>("show_track_point", params, show_track_point_);
 
-    double trail_angle_az = sc::Angles::deg2rad(sc::get<double>("trail_angle_azimuth", params, M_PI));
-    double trail_angle_elev = sc::Angles::deg2rad(sc::get<double>("trail_angle_elevation", params, 0));
+    double trail_angle_az =
+        sc::Angles::deg2rad(sc::get<double>("trail_angle_azimuth", params, M_PI));
+    double trail_angle_elev =
+        sc::Angles::deg2rad(sc::get<double>("trail_angle_elevation", params, 0));
     aa_angle_az_ = Eigen::AngleAxisd(trail_angle_az, Eigen::Vector3d::UnitZ());
     aa_angle_elev_ = Eigen::AngleAxisd(trail_angle_elev, Eigen::Vector3d::UnitY());
 }
@@ -84,8 +83,8 @@ bool TrailMS::step_autonomy(double t, double dt) {
     // center. Rotate a unit vector pointing towards the x-axis around the
     // z-axis by the trail angle. Then rotate that vector by the other entity's
     // orientation.
-    Eigen::Vector3d direction = trail_state->quat() * aa_angle_elev_ *
-        aa_angle_az_ * Eigen::Vector3d::UnitX();
+    Eigen::Vector3d direction =
+        trail_state->quat() * aa_angle_elev_ * aa_angle_az_ * Eigen::Vector3d::UnitX();
 
     // Compute the position of the desired trail point
     Eigen::Vector3d trail_point = trail_state->pos() + direction * trail_range_;
@@ -111,8 +110,7 @@ bool TrailMS::step_autonomy(double t, double dt) {
 
     if (show_track_point_) {
         if (sphere_shape_ == nullptr) {
-            sphere_shape_ = sc::shape::make_sphere(trail_point, 0.5,
-                                                   Eigen::Vector3d(0, 0, 255));
+            sphere_shape_ = sc::shape::make_sphere(trail_point, 0.5, Eigen::Vector3d(0, 0, 255));
         }
         sc::set(sphere_shape_->mutable_sphere()->mutable_center(), trail_point);
         draw_shape(sphere_shape_);
@@ -120,5 +118,5 @@ bool TrailMS::step_autonomy(double t, double dt) {
 
     return true;
 }
-} // namespace autonomy
-} // namespace scrimmage
+}  // namespace autonomy
+}  // namespace scrimmage
diff --git a/src/plugins/autonomy/TrajectoryRecordPlayback/TrajectoryRecordPlayback.cpp b/src/plugins/autonomy/TrajectoryRecordPlayback/TrajectoryRecordPlayback.cpp
index faebff93c0..ed16a20972 100644
--- a/src/plugins/autonomy/TrajectoryRecordPlayback/TrajectoryRecordPlayback.cpp
+++ b/src/plugins/autonomy/TrajectoryRecordPlayback/TrajectoryRecordPlayback.cpp
@@ -29,13 +29,13 @@
  * A Long description goes here.
  *
  */
-#include <scrimmage/plugin_manager/RegisterPlugin.h>
 #include <scrimmage/entity/Entity.h>
 #include <scrimmage/math/State.h>
-#include <scrimmage/parse/ParseUtils.h>
 #include <scrimmage/parse/MissionParse.h>
-#include <scrimmage/plugins/autonomy/TrajectoryRecordPlayback/TrajectoryRecordPlayback.h>
+#include <scrimmage/parse/ParseUtils.h>
+#include <scrimmage/plugin_manager/RegisterPlugin.h>
 #include <scrimmage/plugins/autonomy/TrajectoryRecordPlayback/TrajectoryPoint.h>
+#include <scrimmage/plugins/autonomy/TrajectoryRecordPlayback/TrajectoryRecordPlayback.h>
 
 #include <iostream>
 
@@ -44,28 +44,23 @@ using std::endl;
 
 namespace sc = scrimmage;
 
-REGISTER_PLUGIN(scrimmage::Autonomy,
-                scrimmage::autonomy::TrajectoryRecordPlayback,
+REGISTER_PLUGIN(scrimmage::Autonomy, scrimmage::autonomy::TrajectoryRecordPlayback,
                 TrajectoryRecordPlayback_plugin)
 
 namespace scrimmage {
 namespace autonomy {
 
 TrajectoryRecordPlayback::TrajectoryRecordPlayback()
-    : enable_playback_(false), remove_at_end_(true),
-      trajectory_filename_("trajectory.txt") { }
+    : enable_playback_(false), remove_at_end_(true), trajectory_filename_("trajectory.txt") {
+}
 
 TrajectoryRecordPlayback::~TrajectoryRecordPlayback() {
     csv_.close_output();
 }
 
-void TrajectoryRecordPlayback::init(std::map<std::string,
-                                    std::string> &params) {
-
-    trajectory_filename_ = sc::get<std::string>("trajectory_filename", params,
-                                                "trajectory.txt");
-    trajectory_filename_ = parent_->mp()->root_log_dir() + "/" +
-        trajectory_filename_;
+void TrajectoryRecordPlayback::init(std::map<std::string, std::string>& params) {
+    trajectory_filename_ = sc::get<std::string>("trajectory_filename", params, "trajectory.txt");
+    trajectory_filename_ = parent_->mp()->root_log_dir() + "/" + trajectory_filename_;
 
     remove_at_end_ = sc::get<bool>("remove_at_end", params, true);
 
@@ -77,31 +72,26 @@ void TrajectoryRecordPlayback::init(std::map<std::string,
             cout << "Unable to open log file" << endl;
             return;
         }
-        csv_.set_column_headers("t, x, y, z, vx, vy, vz,"
-                                "roll, pitch, yaw,"
-                                "x_d, y_d, z_d, vx_d, vy_d, vz_d,"
-                                "roll_d, pitch_d, yaw_d");
+        csv_.set_column_headers(
+            "t, x, y, z, vx, vy, vz,"
+            "roll, pitch, yaw,"
+            "x_d, y_d, z_d, vx_d, vy_d, vz_d,"
+            "roll_d, pitch_d, yaw_d");
 
     } else {
         this->set_is_controlling(true);
 
         if (!csv_.read_csv(trajectory_filename_)) {
-            cout << "Failed to read CSV file: " << trajectory_filename_
-                 << endl;
+            cout << "Failed to read CSV file: " << trajectory_filename_ << endl;
         }
 
         for (size_t r = 0; r < csv_.rows(); r++) {
             scrimmage::State desired_state;
-            desired_state.pos() << csv_.at(r, "x_d"),
-                csv_.at(r, "y_d"),
-                csv_.at(r, "z_d");
+            desired_state.pos() << csv_.at(r, "x_d"), csv_.at(r, "y_d"), csv_.at(r, "z_d");
 
-            desired_state.vel() << csv_.at(r, "vx_d"),
-                csv_.at(r, "vy_d"),
-                csv_.at(r, "vz_d");
+            desired_state.vel() << csv_.at(r, "vx_d"), csv_.at(r, "vy_d"), csv_.at(r, "vz_d");
 
-            scrimmage::Quaternion quat(csv_.at(r, "roll_d"),
-                                       csv_.at(r, "pitch_d"),
+            scrimmage::Quaternion quat(csv_.at(r, "roll_d"), csv_.at(r, "pitch_d"),
                                        csv_.at(r, "yaw_d"));
 
             desired_state.quat() = quat;
@@ -118,31 +108,30 @@ void TrajectoryRecordPlayback::init(std::map<std::string,
 bool TrajectoryRecordPlayback::step_autonomy(double t, double dt) {
     if (!enable_playback_) {
         // Get previous autonomy's desired_state
-        auto it = std::find_if(parent_->autonomies().rbegin(),
-                               parent_->autonomies().rend(),
-            [&](auto autonomy) {return autonomy->get_is_controlling();});
+        auto it = std::find_if(parent_->autonomies().rbegin(), parent_->autonomies().rend(),
+                               [&](auto autonomy) { return autonomy->get_is_controlling(); });
 
         csv_.append(sc::CSV::Pairs{
-                {"t", t},
-                {"x", state_->pos()(0)},
-                {"y", state_->pos()(1)},
-                {"z", state_->pos()(2)},
-                {"vx", state_->vel()(0)},
-                {"vy", state_->vel()(1)},
-                {"vz", state_->vel()(2)},
-                {"roll", state_->quat().roll()},
-                {"pitch", state_->quat().pitch()},
-                {"yaw", state_->quat().yaw()},
-                {"x_d", (*(*it)->desired_state()).pos()(0)},
-                {"y_d", (*(*it)->desired_state()).pos()(1)},
-                {"z_d", (*(*it)->desired_state()).pos()(2)},
-                {"vx_d", (*(*it)->desired_state()).vel()(0)},
-                {"vy_d", (*(*it)->desired_state()).vel()(1)},
-                {"vz_d", (*(*it)->desired_state()).vel()(2)},
-                {"roll_d", (*(*it)->desired_state()).quat().roll()},
-                {"pitch_d", (*(*it)->desired_state()).quat().pitch()},
-                {"yaw_d", (*(*it)->desired_state()).quat().yaw()},
-            });
+            {"t", t},
+            {"x", state_->pos()(0)},
+            {"y", state_->pos()(1)},
+            {"z", state_->pos()(2)},
+            {"vx", state_->vel()(0)},
+            {"vy", state_->vel()(1)},
+            {"vz", state_->vel()(2)},
+            {"roll", state_->quat().roll()},
+            {"pitch", state_->quat().pitch()},
+            {"yaw", state_->quat().yaw()},
+            {"x_d", (*(*it)->desired_state()).pos()(0)},
+            {"y_d", (*(*it)->desired_state()).pos()(1)},
+            {"z_d", (*(*it)->desired_state()).pos()(2)},
+            {"vx_d", (*(*it)->desired_state()).vel()(0)},
+            {"vy_d", (*(*it)->desired_state()).vel()(1)},
+            {"vz_d", (*(*it)->desired_state()).vel()(2)},
+            {"roll_d", (*(*it)->desired_state()).quat().roll()},
+            {"pitch_d", (*(*it)->desired_state()).quat().pitch()},
+            {"yaw_d", (*(*it)->desired_state()).quat().yaw()},
+        });
 
         return true;
     }
@@ -165,5 +154,5 @@ bool TrajectoryRecordPlayback::step_autonomy(double t, double dt) {
 
     return true;
 }
-} // namespace autonomy
-} // namespace scrimmage
+}  // namespace autonomy
+}  // namespace scrimmage
diff --git a/src/plugins/autonomy/WaypointDispatcher/WaypointDispatcher.cpp b/src/plugins/autonomy/WaypointDispatcher/WaypointDispatcher.cpp
index fe29a5c4ff..2ff4837962 100644
--- a/src/plugins/autonomy/WaypointDispatcher/WaypointDispatcher.cpp
+++ b/src/plugins/autonomy/WaypointDispatcher/WaypointDispatcher.cpp
@@ -30,56 +30,52 @@
  *
  */
 
-#include <scrimmage/plugins/autonomy/WaypointDispatcher/WaypointDispatcher.h>
-
-#include <scrimmage/plugin_manager/RegisterPlugin.h>
 #include <scrimmage/entity/Entity.h>
 #include <scrimmage/math/State.h>
 #include <scrimmage/parse/ParseUtils.h>
+#include <scrimmage/plugin_manager/RegisterPlugin.h>
+#include <scrimmage/plugins/autonomy/WaypointDispatcher/WaypointDispatcher.h>
+#include <scrimmage/proto/ProtoConversions.h>
+#include <scrimmage/proto/Shape.pb.h>
 #include <scrimmage/pubsub/Publisher.h>
 #include <scrimmage/pubsub/Subscriber.h>
-#include <scrimmage/proto/Shape.pb.h>
-#include <scrimmage/proto/ProtoConversions.h>
 
+#include <GeographicLib/LocalCartesian.hpp>
 #include <iostream>
 #include <limits>
 
-#include <GeographicLib/LocalCartesian.hpp>
-
 using std::cout;
 using std::endl;
 
 namespace sc = scrimmage;
 
-REGISTER_PLUGIN(scrimmage::Autonomy,
-                scrimmage::autonomy::WaypointDispatcher,
+REGISTER_PLUGIN(scrimmage::Autonomy, scrimmage::autonomy::WaypointDispatcher,
                 WaypointDispatcher_plugin)
 
 namespace scrimmage {
 namespace autonomy {
 
-Eigen::Vector3d WaypointDispatcher::lla_to_xyz(const Waypoint &wpt) {
+Eigen::Vector3d WaypointDispatcher::lla_to_xyz(const Waypoint& wpt) {
     Eigen::Vector3d xyz;
-    parent_->projection()->Forward(
-        wpt.latitude(), wpt.longitude(), wpt.altitude(), xyz(0), xyz(1), xyz(2));
+    parent_->projection()->Forward(wpt.latitude(), wpt.longitude(), wpt.altitude(), xyz(0), xyz(1),
+                                   xyz(2));
     return xyz;
 }
 
-void WaypointDispatcher::init(std::map<std::string, std::string> &params) {
+void WaypointDispatcher::init(std::map<std::string, std::string>& params) {
     lead_distance_ = sc::get<double>("lead_distance", params, lead_distance_);
     filter_dist_ = sc::get<double>("filter_dist", params, filter_dist_);
     show_shapes_ = sc::get<bool>("show_shapes", params, show_shapes_);
     tolerance_in_2d_ = sc::get<bool>("tolerance_in_2d", params, tolerance_in_2d_);
 
-    std::string waypointlist_network = get<std::string>(
-        "waypointlist_network", params, "GlobalNetwork");
-    std::string waypoint_network = sc::get<std::string>(
-        "waypoint_network", params, "LocalNetwork");
+    std::string waypointlist_network =
+        get<std::string>("waypointlist_network", params, "GlobalNetwork");
+    std::string waypoint_network = sc::get<std::string>("waypoint_network", params, "LocalNetwork");
 
     wp_pub_ = advertise(waypoint_network, "Waypoint");
     wp_pub_status_ = advertise(waypoint_network, "WaypointStatus");
 
-    auto wp_list_cb = [&] (scrimmage::MessagePtr<WaypointList> msg) {
+    auto wp_list_cb = [&](scrimmage::MessagePtr<WaypointList> msg) {
         // Received a new waypoint list, reset
         wp_list_ = msg->data;
         wp_it_ = wp_list_.waypoints().begin();
@@ -89,7 +85,7 @@ void WaypointDispatcher::init(std::map<std::string, std::string> &params) {
 
         // Filter the waypoint list to ensure that two waypoints in succession
         // are not equal, otherwise, the line following will produce NaN
-        auto is_close = [&](auto &wpt1, auto &wpt2) {
+        auto is_close = [&](auto& wpt1, auto& wpt2) {
             return (this->lla_to_xyz(wpt1) - this->lla_to_xyz(wpt2)).norm() < filter_dist_;
         };
         wp_list_.waypoints().unique(is_close);
@@ -105,8 +101,7 @@ bool WaypointDispatcher::step_autonomy(double t, double dt) {
     Waypoint curr_wp_lla = *wp_it_;
 
     Eigen::Vector3d wp;
-    parent_->projection()->Forward(curr_wp_lla.latitude(),
-                                   curr_wp_lla.longitude(),
+    parent_->projection()->Forward(curr_wp_lla.latitude(), curr_wp_lla.longitude(),
                                    curr_wp_lla.altitude(), wp(0), wp(1), wp(2));
 
     Eigen::Vector3d prev_wp;
@@ -115,8 +110,7 @@ bool WaypointDispatcher::step_autonomy(double t, double dt) {
     } else {
         Waypoint prev_wp_lla = *prev_wp_it_;
         parent_->projection()->Forward(prev_wp_lla.latitude(), prev_wp_lla.longitude(),
-                                       prev_wp_lla.altitude(), prev_wp(0),
-                                       prev_wp(1), prev_wp(2));
+                                       prev_wp_lla.altitude(), prev_wp(0), prev_wp(1), prev_wp(2));
     }
 
     Eigen::Vector3d wp_line = wp - prev_wp;
@@ -124,8 +118,7 @@ bool WaypointDispatcher::step_autonomy(double t, double dt) {
 
     // Project wp_to_own_line onto line between waypoints (wp_line)
     double dot_prod = wp_line.dot(wp_to_own_line);
-    Eigen::Vector3d proj =  dot_prod / pow(wp_line.norm(), 1) *
-        wp_line.normalized();
+    Eigen::Vector3d proj = dot_prod / pow(wp_line.norm(), 1) * wp_line.normalized();
 
     Eigen::Vector3d track_point = prev_wp + proj + wp_line.normalized() * lead_distance_;
 
@@ -137,7 +130,7 @@ bool WaypointDispatcher::step_autonomy(double t, double dt) {
         track_point = wp;
     }
 
-    auto draw_sphere = [&](auto &sphere, auto &pt, double r, double g, double b) {
+    auto draw_sphere = [&](auto& sphere, auto& pt, double r, double g, double b) {
         sphere->set_persistent(true);
         sc::set(sphere->mutable_color(), r, g, b);
         sphere->set_opacity(1.0);
@@ -154,8 +147,7 @@ bool WaypointDispatcher::step_autonomy(double t, double dt) {
 
     // convert track_point to lat/lon and Publish track_point!
     double lat, lon, alt;
-    parent_->projection()->Reverse(track_point(0), track_point(1),
-                                   track_point(2), lat, lon, alt);
+    parent_->projection()->Reverse(track_point(0), track_point(1), track_point(2), lat, lon, alt);
 
     auto wp_msg = std::make_shared<sc::Message<Waypoint>>();
     wp_msg->data = Waypoint(lat, lon, alt);
@@ -163,52 +155,50 @@ bool WaypointDispatcher::step_autonomy(double t, double dt) {
     wp_pub_->publish(wp_msg);
 
     // check if within waypoint tolerance
-    double dist = tolerance_in_2d_ ?
-            (state_->pos().head<2>() - wp.head<2>()).norm() :
-            (state_->pos() - wp).norm();
+    double dist = tolerance_in_2d_ ? (state_->pos().head<2>() - wp.head<2>()).norm()
+                                   : (state_->pos() - wp).norm();
     if (dist <= curr_wp_lla.position_tolerance()) {
-
         bool done = false;
         switch (wp_list_.mode()) {
-        case WaypointList::WaypointMode::follow_once :
-            prev_wp_it_ = wp_it_;
-            ++wp_it_;
-
-            if (wp_it_ == wp_list_.waypoints().end()) {
-                wp_it_ = prev_wp_it_;
-                done = true;
-            }
-            break;
-
-        case WaypointList::WaypointMode::back_and_forth :
-            prev_wp_it_ = wp_it_;
-            wp_it_ = returning_stage_ ? --wp_it_ : ++wp_it_;
-
-            if (wp_it_ == wp_list_.waypoints().begin()) {
-                returning_stage_ = false;
-            } else if (wp_it_ == wp_list_.waypoints().end()) {
-                wp_it_ = prev_wp_it_;
-                returning_stage_ = true;
-            }
-            break;
-
-        case WaypointList::WaypointMode::loiter :
-            // TODO : Not implemented yet
-            break;
-
-        case WaypointList::WaypointMode::racetrack :
-            // Continuous repeat of waypoints
-            prev_wp_it_ = wp_it_;
-            ++wp_it_;
-
-            if (wp_it_ == wp_list_.waypoints().end()) {
-                wp_it_ = wp_list_.waypoints().begin();
-            }
-            break;
-
-        default :
-            std::string msg = "Waypoint mode not defined yet.";
-            throw std::runtime_error(msg);
+            case WaypointList::WaypointMode::follow_once:
+                prev_wp_it_ = wp_it_;
+                ++wp_it_;
+
+                if (wp_it_ == wp_list_.waypoints().end()) {
+                    wp_it_ = prev_wp_it_;
+                    done = true;
+                }
+                break;
+
+            case WaypointList::WaypointMode::back_and_forth:
+                prev_wp_it_ = wp_it_;
+                wp_it_ = returning_stage_ ? --wp_it_ : ++wp_it_;
+
+                if (wp_it_ == wp_list_.waypoints().begin()) {
+                    returning_stage_ = false;
+                } else if (wp_it_ == wp_list_.waypoints().end()) {
+                    wp_it_ = prev_wp_it_;
+                    returning_stage_ = true;
+                }
+                break;
+
+            case WaypointList::WaypointMode::loiter:
+                // TODO : Not implemented yet
+                break;
+
+            case WaypointList::WaypointMode::racetrack:
+                // Continuous repeat of waypoints
+                prev_wp_it_ = wp_it_;
+                ++wp_it_;
+
+                if (wp_it_ == wp_list_.waypoints().end()) {
+                    wp_it_ = wp_list_.waypoints().begin();
+                }
+                break;
+
+            default:
+                std::string msg = "Waypoint mode not defined yet.";
+                throw std::runtime_error(msg);
         }
 
         auto msg = std::make_shared<sc::Message<std::tuple<size_t, size_t, bool>>>();
@@ -219,5 +209,5 @@ bool WaypointDispatcher::step_autonomy(double t, double dt) {
 
     return true;
 }
-} // namespace autonomy
-} // namespace scrimmage
+}  // namespace autonomy
+}  // namespace scrimmage
diff --git a/src/plugins/autonomy/WaypointGenerator/WaypointGenerator.cpp b/src/plugins/autonomy/WaypointGenerator/WaypointGenerator.cpp
index ced53a0f6f..e50db62e66 100644
--- a/src/plugins/autonomy/WaypointGenerator/WaypointGenerator.cpp
+++ b/src/plugins/autonomy/WaypointGenerator/WaypointGenerator.cpp
@@ -30,34 +30,31 @@
  *
  */
 
+#include <scrimmage/common/VariableIO.h>
 #include <scrimmage/common/Waypoint.h>
-#include <scrimmage/plugins/autonomy/WaypointGenerator/WaypointGenerator.h>
-#include <scrimmage/plugins/autonomy/WaypointGenerator/WaypointList.h>
-
-#include <scrimmage/plugin_manager/RegisterPlugin.h>
 #include <scrimmage/entity/Entity.h>
-#include <scrimmage/math/State.h>
 #include <scrimmage/math/Angles.h>
+#include <scrimmage/math/State.h>
 #include <scrimmage/parse/ParseUtils.h>
-#include <scrimmage/pubsub/Message.h>
-#include <scrimmage/pubsub/Publisher.h>
+#include <scrimmage/plugin_manager/RegisterPlugin.h>
+#include <scrimmage/plugins/autonomy/WaypointGenerator/WaypointGenerator.h>
+#include <scrimmage/plugins/autonomy/WaypointGenerator/WaypointList.h>
 #include <scrimmage/proto/ProtoConversions.h>
 #include <scrimmage/proto/Shape.pb.h>
-#include <scrimmage/common/VariableIO.h>
+#include <scrimmage/pubsub/Message.h>
+#include <scrimmage/pubsub/Publisher.h>
 
+#include <GeographicLib/LocalCartesian.hpp>
 #include <iostream>
 #include <limits>
 
-#include <GeographicLib/LocalCartesian.hpp>
-
 using std::cout;
 using std::endl;
 
 namespace sc = scrimmage;
 namespace sp = scrimmage_proto;
 
-REGISTER_PLUGIN(scrimmage::Autonomy,
-                scrimmage::autonomy::WaypointGenerator,
+REGISTER_PLUGIN(scrimmage::Autonomy, scrimmage::autonomy::WaypointGenerator,
                 WaypointGenerator_plugin)
 
 namespace scrimmage {
@@ -66,7 +63,7 @@ namespace autonomy {
 WaypointGenerator::WaypointGenerator() {
 }
 
-void WaypointGenerator::init(std::map<std::string, std::string> &params) {
+void WaypointGenerator::init(std::map<std::string, std::string>& params) {
     waypoint_color_ = sc::str2container<std::vector<int>>(
         sc::get<std::string>("waypoint_color", params, "255,0,0"), ",");
 
@@ -94,8 +91,7 @@ void WaypointGenerator::init(std::map<std::string, std::string> &params) {
                 cout << "Invalid waypoint type: " << type << endl;
             } else if (type == "XYZ") {
                 // Convert to XYZ point to GPS
-                parent_->projection()->Reverse(lat, lon, alt,
-                                               lat, lon, alt);
+                parent_->projection()->Reverse(lat, lon, alt, lat, lon, alt);
             }
 
             Waypoint wp(lat, lon, alt);
@@ -162,11 +158,10 @@ bool WaypointGenerator::step_autonomy(double t, double dt) {
     return true;
 }
 
-void WaypointGenerator::draw_waypoints(WaypointList &wp_list) {
+void WaypointGenerator::draw_waypoints(WaypointList& wp_list) {
     for (Waypoint wp : wp_list.waypoints()) {
         double x, y, z;
-        parent_->projection()->Forward(wp.latitude(), wp.longitude(), wp.altitude(),
-                                       x, y, z);
+        parent_->projection()->Forward(wp.latitude(), wp.longitude(), wp.altitude(), x, y, z);
         vars_.output(position_x_idx_, x);
         vars_.output(position_y_idx_, y);
         vars_.output(position_z_idx_, z);
@@ -182,5 +177,5 @@ void WaypointGenerator::draw_waypoints(WaypointList &wp_list) {
         draw_shape(sphere);
     }
 }
-} // namespace autonomy
-} // namespace scrimmage
+}  // namespace autonomy
+}  // namespace scrimmage
diff --git a/src/plugins/autonomy/follow/Follow.cpp b/src/plugins/autonomy/follow/Follow.cpp
index 48b7b92182..53aa86cd5c 100644
--- a/src/plugins/autonomy/follow/Follow.cpp
+++ b/src/plugins/autonomy/follow/Follow.cpp
@@ -35,8 +35,8 @@
 #include <scrimmage/plugin_manager/RegisterPlugin.h>
 #include <scrimmage/plugins/autonomy/follow/Follow.h>
 
-#include <GeographicLib/Geodesic.hpp>
 #include <GeographicLib/Geocentric.hpp>
+#include <GeographicLib/Geodesic.hpp>
 #include <GeographicLib/LocalCartesian.hpp>
 
 REGISTER_PLUGIN(scrimmage::Autonomy, scrimmage::autonomy::Follow, Follow_plugin)
@@ -44,58 +44,59 @@ REGISTER_PLUGIN(scrimmage::Autonomy, scrimmage::autonomy::Follow, Follow_plugin)
 namespace scrimmage {
 namespace autonomy {
 
-Follow::Follow() {}
+Follow::Follow() {
+}
 
-void Follow::init(std::map<std::string, std::string> &params) {
-     desired_state_->vel() = 21 * Eigen::Vector3d::UnitX();
-     desired_state_->quat().set(0, 0, state_->quat().yaw());
-     desired_state_->pos() = state_->pos()(2) * Eigen::Vector3d::UnitZ();
+void Follow::init(std::map<std::string, std::string>& params) {
+    desired_state_->vel() = 21 * Eigen::Vector3d::UnitX();
+    desired_state_->quat().set(0, 0, state_->quat().yaw());
+    desired_state_->pos() = state_->pos()(2) * Eigen::Vector3d::UnitZ();
 }
 
 bool Follow::step_autonomy(double t, double dt) {
-     // // Find the closest contact
-     // const GeographicLib::Geodesic& geod = Geodesic::WGS84();
-     // double min_dist = 1e10;
-     // bool contact_found = false;
-     // std::list<scrimmage::Contact>::iterator min_champ;
-     // for (std::list<scrimmage::Contact>::iterator it = contacts_.begin();
-     //      it != contacts_.end(); it++) {
-     //
-     //      if (it->id().id() != parent_->id().id()) {
-     //           //cout << "Own Lat: " << state_.pose().latitude() << endl;
-     //           //cout << "Own Long: " << state_.pose().longitude() << endl;
-     //           //cout << "Cnt Lat: " << it->state().pose().latitude()<< endl;
-     //           //cout << "Cnt Long: " << it->state().pose().longitude()<< endl;
-     //
-     //           double dist;
-     //           geod.Inverse(state_.pose().latitude(), state_.pose().longitude(),
-     //                        it->state().pose().latitude(), it->state().pose().longitude(),
-     //                        dist);
-     //
-     //           if (dist < min_dist) {
-     //                min_dist = dist;
-     //                min_champ = it;
-     //                contact_found = true;
-     //           }
-     //      }
-     // }
-     //
-     // // Head towards the closet contact if it exists
-     // if (contact_found) {
-     //      Geocentric earth(Constants::WGS84_a(), Constants::WGS84_f());
-     //      // Alternatively: const Geocentric& earth = Geocentric::WGS84();
-     //
-     //      LocalCartesian proj(state_.pose().latitude(),
-     //                          state_.pose().longitude(), 0, earth);
-     //      // Sample forward calculation
-     //      double x, y, z;
-     //      double h = 0;
-     //      proj.Forward(min_champ->state().pose().latitude(), min_champ->state().pose().longitude(), h, x, y, z);
-     //      double heading = atan2(x,y) * 180.0 / scrimmage::PI;
-     //      desired_state_.pose().set_heading(heading);
-     // }
+    // // Find the closest contact
+    // const GeographicLib::Geodesic& geod = Geodesic::WGS84();
+    // double min_dist = 1e10;
+    // bool contact_found = false;
+    // std::list<scrimmage::Contact>::iterator min_champ;
+    // for (std::list<scrimmage::Contact>::iterator it = contacts_.begin();
+    //      it != contacts_.end(); it++) {
+    //
+    //      if (it->id().id() != parent_->id().id()) {
+    //           //cout << "Own Lat: " << state_.pose().latitude() << endl;
+    //           //cout << "Own Long: " << state_.pose().longitude() << endl;
+    //           //cout << "Cnt Lat: " << it->state().pose().latitude()<< endl;
+    //           //cout << "Cnt Long: " << it->state().pose().longitude()<< endl;
+    //
+    //           double dist;
+    //           geod.Inverse(state_.pose().latitude(), state_.pose().longitude(),
+    //                        it->state().pose().latitude(), it->state().pose().longitude(),
+    //                        dist);
+    //
+    //           if (dist < min_dist) {
+    //                min_dist = dist;
+    //                min_champ = it;
+    //                contact_found = true;
+    //           }
+    //      }
+    // }
+    //
+    // // Head towards the closet contact if it exists
+    // if (contact_found) {
+    //      Geocentric earth(Constants::WGS84_a(), Constants::WGS84_f());
+    //      // Alternatively: const Geocentric& earth = Geocentric::WGS84();
+    //
+    //      LocalCartesian proj(state_.pose().latitude(),
+    //                          state_.pose().longitude(), 0, earth);
+    //      // Sample forward calculation
+    //      double x, y, z;
+    //      double h = 0;
+    //      proj.Forward(min_champ->state().pose().latitude(),
+    //      min_champ->state().pose().longitude(), h, x, y, z); double heading = atan2(x,y) * 180.0
+    //      / scrimmage::PI; desired_state_.pose().set_heading(heading);
+    // }
 
-     return true;
+    return true;
 }
-} // namespace autonomy
-} // namespace scrimmage
+}  // namespace autonomy
+}  // namespace scrimmage
diff --git a/src/plugins/controller/AircraftPIDController/AircraftPIDController.cpp b/src/plugins/controller/AircraftPIDController/AircraftPIDController.cpp
index 4ea1bd4a1b..99f2b1470b 100644
--- a/src/plugins/controller/AircraftPIDController/AircraftPIDController.cpp
+++ b/src/plugins/controller/AircraftPIDController/AircraftPIDController.cpp
@@ -30,29 +30,26 @@
  *
  */
 
-#include <scrimmage/plugins/controller/AircraftPIDController/AircraftPIDController.h>
-
-#include <scrimmage/plugin_manager/RegisterPlugin.h>
+#include <scrimmage/common/Time.h>
+#include <scrimmage/common/Utilities.h>
 #include <scrimmage/entity/Entity.h>
-#include <scrimmage/math/State.h>
 #include <scrimmage/math/Angles.h>
-#include <scrimmage/common/Utilities.h>
-#include <scrimmage/common/Time.h>
+#include <scrimmage/math/State.h>
 #include <scrimmage/parse/ParseUtils.h>
+#include <scrimmage/plugin_manager/RegisterPlugin.h>
+#include <scrimmage/plugins/controller/AircraftPIDController/AircraftPIDController.h>
 
+#include <boost/algorithm/clamp.hpp>
 #include <iostream>
 #include <limits>
 
-#include <boost/algorithm/clamp.hpp>
-
+using boost::algorithm::clamp;
 using std::cout;
 using std::endl;
-using boost::algorithm::clamp;
 
 namespace sc = scrimmage;
 
-REGISTER_PLUGIN(scrimmage::Controller,
-                scrimmage::controller::AircraftPIDController,
+REGISTER_PLUGIN(scrimmage::Controller, scrimmage::controller::AircraftPIDController,
                 AircraftPIDController_plugin)
 
 namespace scrimmage {
@@ -61,21 +58,24 @@ namespace controller {
 AircraftPIDController::AircraftPIDController() {
 }
 
-void AircraftPIDController::init(std::map<std::string, std::string> &params) {
+void AircraftPIDController::init(std::map<std::string, std::string>& params) {
     use_roll_control_ = sc::get<bool>("use_roll_control", params, use_roll_control_);
 
     // Setup input variables
     if (use_roll_control_) {
-        desired_roll_idx_ = vars_.declare(VariableIO::Type::desired_roll, VariableIO::Direction::In);
+        desired_roll_idx_ =
+            vars_.declare(VariableIO::Type::desired_roll, VariableIO::Direction::In);
     } else {
-        desired_heading_idx_ = vars_.declare(VariableIO::Type::desired_heading, VariableIO::Direction::In);
+        desired_heading_idx_ =
+            vars_.declare(VariableIO::Type::desired_heading, VariableIO::Direction::In);
 
         // Outer loop heading PID
         if (!heading_pid_.init(params["heading_pid"], true)) {
             std::cout << "Failed to set heading PID" << std::endl;
         }
     }
-    desired_altitude_idx_ = vars_.declare(VariableIO::Type::desired_altitude, VariableIO::Direction::In);
+    desired_altitude_idx_ =
+        vars_.declare(VariableIO::Type::desired_altitude, VariableIO::Direction::In);
     desired_speed_idx_ = vars_.declare(VariableIO::Type::desired_speed, VariableIO::Direction::In);
 
     // Setup output variables
@@ -140,5 +140,5 @@ bool AircraftPIDController::step(double t, double dt) {
 
     return true;
 }
-} // namespace controller
-} // namespace scrimmage
+}  // namespace controller
+}  // namespace scrimmage
diff --git a/src/plugins/controller/AircraftToSingleIntegratorController/AircraftToSingleIntegratorController.cpp b/src/plugins/controller/AircraftToSingleIntegratorController/AircraftToSingleIntegratorController.cpp
index e9f1bb440c..fe2fe321ca 100644
--- a/src/plugins/controller/AircraftToSingleIntegratorController/AircraftToSingleIntegratorController.cpp
+++ b/src/plugins/controller/AircraftToSingleIntegratorController/AircraftToSingleIntegratorController.cpp
@@ -32,9 +32,9 @@
 
 #include <scrimmage/plugin_manager/RegisterPlugin.h>
 #include <scrimmage/plugins/controller/AircraftToSingleIntegratorController/AircraftToSingleIntegratorController.h>
-REGISTER_PLUGIN(scrimmage::Controller, scrimmage::controller::AircraftToSingleIntegratorController, AircraftToSingleIntegratorController_plugin)
+REGISTER_PLUGIN(scrimmage::Controller, scrimmage::controller::AircraftToSingleIntegratorController,
+                AircraftToSingleIntegratorController_plugin)
 
 namespace scrimmage {
-namespace controller {
-} // namespace controller
-} // namespace scrimmage
+namespace controller {}  // namespace controller
+}  // namespace scrimmage
diff --git a/src/plugins/controller/DirectController/DirectController.cpp b/src/plugins/controller/DirectController/DirectController.cpp
index 8eb52a7901..64bd9f7b72 100644
--- a/src/plugins/controller/DirectController/DirectController.cpp
+++ b/src/plugins/controller/DirectController/DirectController.cpp
@@ -30,14 +30,13 @@
  *
  */
 
-#include <scrimmage/plugins/controller/DirectController/DirectController.h>
-
-#include <scrimmage/plugin_manager/RegisterPlugin.h>
-#include <scrimmage/entity/Entity.h>
-#include <scrimmage/math/State.h>
 #include <scrimmage/common/Utilities.h>
 #include <scrimmage/common/VariableIO.h>
+#include <scrimmage/entity/Entity.h>
+#include <scrimmage/math/State.h>
 #include <scrimmage/parse/ParseUtils.h>
+#include <scrimmage/plugin_manager/RegisterPlugin.h>
+#include <scrimmage/plugins/controller/DirectController/DirectController.h>
 
 #include <iostream>
 #include <limits>
@@ -47,17 +46,16 @@ using std::endl;
 
 namespace sc = scrimmage;
 
-REGISTER_PLUGIN(scrimmage::Controller,
-                scrimmage::controller::DirectController,
+REGISTER_PLUGIN(scrimmage::Controller, scrimmage::controller::DirectController,
                 DirectController_plugin)
 
 namespace scrimmage {
 namespace controller {
 
-void DirectController::init(std::map<std::string, std::string> &params) {
+void DirectController::init(std::map<std::string, std::string>& params) {
     // Discover the output variables that point to the input variables for the
     // motion model. Setup input variables that mirror the output variables.
-    for (auto &kv : vars_.output_variable_index()) {
+    for (auto& kv : vars_.output_variable_index()) {
         int out_idx = vars_.declare(kv.first, VariableIO::Direction::Out);
         int in_idx = vars_.declare(kv.first, VariableIO::Direction::In);
         io_map_[out_idx] = in_idx;
@@ -66,10 +64,10 @@ void DirectController::init(std::map<std::string, std::string> &params) {
 
 bool DirectController::step(double t, double dt) {
     // Copy over variableIO data
-    for (auto &kv : io_map_) {
+    for (auto& kv : io_map_) {
         vars_.output(kv.first, vars_.input(kv.second));
     }
     return true;
 }
-} // namespace controller
-} // namespace scrimmage
+}  // namespace controller
+}  // namespace scrimmage
diff --git a/src/plugins/controller/DoubleIntegratorControllerVelYaw/DoubleIntegratorControllerVelYaw.cpp b/src/plugins/controller/DoubleIntegratorControllerVelYaw/DoubleIntegratorControllerVelYaw.cpp
index 1649aae946..9f604af196 100644
--- a/src/plugins/controller/DoubleIntegratorControllerVelYaw/DoubleIntegratorControllerVelYaw.cpp
+++ b/src/plugins/controller/DoubleIntegratorControllerVelYaw/DoubleIntegratorControllerVelYaw.cpp
@@ -30,10 +30,10 @@
  *
  */
 
-#include <scrimmage/plugin_manager/RegisterPlugin.h>
 #include <scrimmage/common/Utilities.h>
 #include <scrimmage/math/Angles.h>
 #include <scrimmage/parse/ParseUtils.h>
+#include <scrimmage/plugin_manager/RegisterPlugin.h>
 #include <scrimmage/plugins/controller/DoubleIntegratorControllerVelYaw/DoubleIntegratorControllerVelYaw.h>
 
 #include <iostream>
@@ -42,15 +42,17 @@ using std::endl;
 
 namespace sc = scrimmage;
 
-REGISTER_PLUGIN(scrimmage::Controller, scrimmage::controller::DoubleIntegratorControllerVelYaw, DoubleIntegratorControllerVelYaw_plugin)
+REGISTER_PLUGIN(scrimmage::Controller, scrimmage::controller::DoubleIntegratorControllerVelYaw,
+                DoubleIntegratorControllerVelYaw_plugin)
 
 namespace scrimmage {
 namespace controller {
 
-void DoubleIntegratorControllerVelYaw::init(std::map<std::string, std::string> &params) {
+void DoubleIntegratorControllerVelYaw::init(std::map<std::string, std::string>& params) {
     desired_alt_idx_ = vars_.declare(VariableIO::Type::desired_altitude, VariableIO::Direction::In);
     desired_speed_idx_ = vars_.declare(VariableIO::Type::desired_speed, VariableIO::Direction::In);
-    desired_heading_idx_ = vars_.declare(VariableIO::Type::desired_heading, VariableIO::Direction::In);
+    desired_heading_idx_ =
+        vars_.declare(VariableIO::Type::desired_heading, VariableIO::Direction::In);
 
     acc_x_idx_ = vars_.declare(VariableIO::Type::acceleration_x, VariableIO::Direction::Out);
     acc_y_idx_ = vars_.declare(VariableIO::Type::acceleration_y, VariableIO::Direction::Out);
@@ -58,18 +60,15 @@ void DoubleIntegratorControllerVelYaw::init(std::map<std::string, std::string> &
     turn_rate_idx_ = vars_.declare(VariableIO::Type::turn_rate, VariableIO::Direction::Out);
 
     if (!sc::set_pid_gains(yaw_pid_, params["yaw_pid"], true)) {
-        cout << "Failed to set DoubleIntegratorControllerVewYaw yaw gains"
-             << endl;
+        cout << "Failed to set DoubleIntegratorControllerVewYaw yaw gains" << endl;
     }
 
     if (!sc::set_pid_gains(speed_pid_, params["speed_pid"])) {
-        cout << "Failed to set DoubleIntegratorControllerVewYaw speed pid"
-             << endl;
+        cout << "Failed to set DoubleIntegratorControllerVewYaw speed pid" << endl;
     }
 
     if (!sc::set_pid_gains(alt_pid_, params["alt_pid"])) {
-        cout << "Failed to set DoubleIntegratorControllerVewYaw alt pid"
-             << endl;
+        cout << "Failed to set DoubleIntegratorControllerVewYaw alt pid" << endl;
     }
 }
 
@@ -96,5 +95,5 @@ bool DoubleIntegratorControllerVelYaw::step(double t, double dt) {
 
     return true;
 }
-} // namespace controller
-} // namespace scrimmage
+}  // namespace controller
+}  // namespace scrimmage
diff --git a/src/plugins/controller/DoubleIntegratorControllerWaypoint/DoubleIntegratorControllerWaypoint.cpp b/src/plugins/controller/DoubleIntegratorControllerWaypoint/DoubleIntegratorControllerWaypoint.cpp
index 403c175d4f..35bb55f9f0 100644
--- a/src/plugins/controller/DoubleIntegratorControllerWaypoint/DoubleIntegratorControllerWaypoint.cpp
+++ b/src/plugins/controller/DoubleIntegratorControllerWaypoint/DoubleIntegratorControllerWaypoint.cpp
@@ -30,29 +30,32 @@
  *
  */
 
-#include <scrimmage/plugin_manager/RegisterPlugin.h>
 #include <scrimmage/common/Utilities.h>
 #include <scrimmage/parse/ParseUtils.h>
+#include <scrimmage/plugin_manager/RegisterPlugin.h>
 #include <scrimmage/plugins/controller/DoubleIntegratorControllerWaypoint/DoubleIntegratorControllerWaypoint.h>
 
 #include <iostream>
 
-REGISTER_PLUGIN(scrimmage::Controller, scrimmage::controller::DoubleIntegratorControllerWaypoint, DoubleIntegratorControllerWaypoint_plugin)
+REGISTER_PLUGIN(scrimmage::Controller, scrimmage::controller::DoubleIntegratorControllerWaypoint,
+                DoubleIntegratorControllerWaypoint_plugin)
 
 namespace scrimmage {
 namespace controller {
 
-void DoubleIntegratorControllerWaypoint::init(std::map<std::string, std::string> &params) {
+void DoubleIntegratorControllerWaypoint::init(std::map<std::string, std::string>& params) {
     std::vector<double> gain;
     if (!scrimmage::str2container(params.at("gain"), ",", gain, 2)) {
-        std::cout << "warning: did not get gain properly in DoubleIntegratorControllerWaypoint" << std::endl;
+        std::cout << "warning: did not get gain properly in DoubleIntegratorControllerWaypoint"
+                  << std::endl;
     } else {
         gain_ << gain[0], gain[1];
     }
 
     desired_alt_idx_ = vars_.declare(VariableIO::Type::desired_altitude, VariableIO::Direction::In);
     desired_speed_idx_ = vars_.declare(VariableIO::Type::desired_speed, VariableIO::Direction::In);
-    desired_heading_idx_ = vars_.declare(VariableIO::Type::desired_heading, VariableIO::Direction::In);
+    desired_heading_idx_ =
+        vars_.declare(VariableIO::Type::desired_heading, VariableIO::Direction::In);
 
     acc_x_idx_ = vars_.declare(VariableIO::Type::acceleration_x, VariableIO::Direction::Out);
     acc_y_idx_ = vars_.declare(VariableIO::Type::acceleration_y, VariableIO::Direction::Out);
@@ -61,8 +64,8 @@ void DoubleIntegratorControllerWaypoint::init(std::map<std::string, std::string>
 }
 
 bool DoubleIntegratorControllerWaypoint::step(double t, double dt) {
-    Eigen::Vector3d acc = -gain_(0) * (state_->pos() - desired_state_->pos())
-        - gain_(1) * state_->vel();
+    Eigen::Vector3d acc =
+        -gain_(0) * (state_->pos() - desired_state_->pos()) - gain_(1) * state_->vel();
 
     vars_.output(acc_x_idx_, acc(0));
     vars_.output(acc_y_idx_, acc(1));
@@ -71,5 +74,5 @@ bool DoubleIntegratorControllerWaypoint::step(double t, double dt) {
 
     return true;
 }
-} // namespace controller
-} // namespace scrimmage
+}  // namespace controller
+}  // namespace scrimmage
diff --git a/src/plugins/controller/FixedWing6DOFControllerPID/FixedWing6DOFControllerPID.cpp b/src/plugins/controller/FixedWing6DOFControllerPID/FixedWing6DOFControllerPID.cpp
index d4a356a02a..4cb2cb4d55 100644
--- a/src/plugins/controller/FixedWing6DOFControllerPID/FixedWing6DOFControllerPID.cpp
+++ b/src/plugins/controller/FixedWing6DOFControllerPID/FixedWing6DOFControllerPID.cpp
@@ -30,26 +30,26 @@
  *
  */
 
-#include <scrimmage/plugins/controller/FixedWing6DOFControllerPID/FixedWing6DOFControllerPID.h>
-#include <scrimmage/plugin_manager/RegisterPlugin.h>
 #include <scrimmage/math/Angles.h>
-#include <vector>
-#include <iostream>
+#include <scrimmage/plugin_manager/RegisterPlugin.h>
+#include <scrimmage/plugins/controller/FixedWing6DOFControllerPID/FixedWing6DOFControllerPID.h>
+
 #include <boost/algorithm/string.hpp>
+#include <iostream>
+#include <vector>
 
 using std::cout;
 using std::endl;
 
-REGISTER_PLUGIN(scrimmage::Controller,
-        scrimmage::controller::FixedWing6DOFControllerPID,
-        FixedWing6DOFControllerPID_plugin)
+REGISTER_PLUGIN(scrimmage::Controller, scrimmage::controller::FixedWing6DOFControllerPID,
+                FixedWing6DOFControllerPID_plugin)
 
 namespace scrimmage {
 namespace controller {
 
 namespace sc = scrimmage;
 
-void set_pid(sc::PID &pid, std::string str, bool is_angle) {
+void set_pid(sc::PID& pid, std::string str, bool is_angle) {
     std::vector<std::string> str_vals;
     boost::split(str_vals, str, boost::is_any_of(","));
 
@@ -72,8 +72,7 @@ void set_pid(sc::PID &pid, std::string str, bool is_angle) {
     }
 }
 
-void FixedWing6DOFControllerPID::init(
-        std::map<std::string, std::string> &params) {
+void FixedWing6DOFControllerPID::init(std::map<std::string, std::string>& params) {
     set_pid(heading_pid_, params["heading_pid"], true);
     set_pid(alt_pid_, params["alt_pid"], false);
     set_pid(vel_pid_, params["vel_pid"], false);
diff --git a/src/plugins/controller/FixedWing6DOFControllerROS/FixedWing6DOFControllerROS.cpp b/src/plugins/controller/FixedWing6DOFControllerROS/FixedWing6DOFControllerROS.cpp
index 97bbed92bc..5d92dc924c 100644
--- a/src/plugins/controller/FixedWing6DOFControllerROS/FixedWing6DOFControllerROS.cpp
+++ b/src/plugins/controller/FixedWing6DOFControllerROS/FixedWing6DOFControllerROS.cpp
@@ -32,21 +32,22 @@
 
 #include <scrimmage/plugin_manager/RegisterPlugin.h>
 #include <scrimmage/plugins/controller/FixedWing6DOFControllerROS/FixedWing6DOFControllerROS.h>
+
 #include <boost/algorithm/string.hpp>
 
-REGISTER_PLUGIN(scrimmage::Controller, scrimmage::controller::FixedWing6DOFControllerROS, FixedWing6DOFControllerROS_plugin)
+REGISTER_PLUGIN(scrimmage::Controller, scrimmage::controller::FixedWing6DOFControllerROS,
+                FixedWing6DOFControllerROS_plugin)
 
 namespace scrimmage {
 namespace controller {
 
 namespace sc = scrimmage;
 
-void FixedWing6DOFControllerROS::init(std::map<std::string, std::string> &params) {
-
+void FixedWing6DOFControllerROS::init(std::map<std::string, std::string>& params) {
     aileron_idx_ = vars_.declare(VariableIO::Type::aileron, VariableIO::Direction::Out);
-	elevator_idx_ = vars_.declare(VariableIO::Type::elevator, VariableIO::Direction::Out);
-	throttle_idx_ = vars_.declare(VariableIO::Type::throttle, VariableIO::Direction::Out);
-	rudder_idx_ = vars_.declare(VariableIO::Type::rudder, VariableIO::Direction::Out);
+    elevator_idx_ = vars_.declare(VariableIO::Type::elevator, VariableIO::Direction::Out);
+    throttle_idx_ = vars_.declare(VariableIO::Type::throttle, VariableIO::Direction::Out);
+    rudder_idx_ = vars_.declare(VariableIO::Type::rudder, VariableIO::Direction::Out);
 
     if (!ros::isInitialized()) {
         int argc = 0;
@@ -77,5 +78,5 @@ bool FixedWing6DOFControllerROS::step(double t, double dt) {
 void FixedWing6DOFControllerROS::cmd_vel_cb(const geometry_msgs::Twist::ConstPtr& msg) {
     cmd_vel_ = *msg;
 }
-} // namespace controller
-} // namespace scrimmage
+}  // namespace controller
+}  // namespace scrimmage
diff --git a/src/plugins/controller/HarmonicOscillatorConstController/HarmonicOscillatorConstController.cpp b/src/plugins/controller/HarmonicOscillatorConstController/HarmonicOscillatorConstController.cpp
index 04aea6aab6..08e105cc19 100644
--- a/src/plugins/controller/HarmonicOscillatorConstController/HarmonicOscillatorConstController.cpp
+++ b/src/plugins/controller/HarmonicOscillatorConstController/HarmonicOscillatorConstController.cpp
@@ -29,14 +29,13 @@
  *
  */
 
-#include <scrimmage/plugins/controller/HarmonicOscillatorConstController/HarmonicOscillatorConstController.h>
-
-#include <scrimmage/plugin_manager/RegisterPlugin.h>
+#include <scrimmage/common/Utilities.h>
+#include <scrimmage/common/VariableIO.h>
 #include <scrimmage/entity/Entity.h>
 #include <scrimmage/math/State.h>
-#include <scrimmage/common/Utilities.h>
 #include <scrimmage/parse/ParseUtils.h>
-#include <scrimmage/common/VariableIO.h>
+#include <scrimmage/plugin_manager/RegisterPlugin.h>
+#include <scrimmage/plugins/controller/HarmonicOscillatorConstController/HarmonicOscillatorConstController.h>
 
 #include <iostream>
 #include <limits>
@@ -46,21 +45,21 @@ using std::endl;
 
 namespace sc = scrimmage;
 
-REGISTER_PLUGIN(scrimmage::Controller,
-                scrimmage::controller::HarmonicOscillatorConstController,
+REGISTER_PLUGIN(scrimmage::Controller, scrimmage::controller::HarmonicOscillatorConstController,
                 HarmonicOscillatorConstController_plugin)
 
 namespace scrimmage {
 namespace controller {
 
-void HarmonicOscillatorConstController::init(std::map<std::string, std::string> &params) {
+void HarmonicOscillatorConstController::init(std::map<std::string, std::string>& params) {
     acceleration_z_ = sc::get<double>("acceleration_z", params, 0);
-    acceleration_z_idx_ = vars_.declare(VariableIO::Type::acceleration_z, VariableIO::Direction::Out);
+    acceleration_z_idx_ =
+        vars_.declare(VariableIO::Type::acceleration_z, VariableIO::Direction::Out);
 }
 
 bool HarmonicOscillatorConstController::step(double t, double dt) {
     vars_.output(acceleration_z_idx_, acceleration_z_);
     return true;
 }
-} // namespace controller
-} // namespace scrimmage
+}  // namespace controller
+}  // namespace scrimmage
diff --git a/src/plugins/controller/JSBSimControlControllerHeadingPID/JSBSimControlControllerHeadingPID.cpp b/src/plugins/controller/JSBSimControlControllerHeadingPID/JSBSimControlControllerHeadingPID.cpp
index 2e97f09631..62e6c08d9a 100644
--- a/src/plugins/controller/JSBSimControlControllerHeadingPID/JSBSimControlControllerHeadingPID.cpp
+++ b/src/plugins/controller/JSBSimControlControllerHeadingPID/JSBSimControlControllerHeadingPID.cpp
@@ -30,17 +30,17 @@
  *
  */
 
-#include <scrimmage/plugin_manager/RegisterPlugin.h>
-#include <scrimmage/plugins/controller/JSBSimControlControllerHeadingPID/JSBSimControlControllerHeadingPID.h>
 #include <scrimmage/common/Utilities.h>
-#include <scrimmage/parse/ParseUtils.h>
 #include <scrimmage/math/State.h>
-
-#include <iostream>
+#include <scrimmage/parse/ParseUtils.h>
+#include <scrimmage/plugin_manager/RegisterPlugin.h>
+#include <scrimmage/plugins/controller/JSBSimControlControllerHeadingPID/JSBSimControlControllerHeadingPID.h>
 
 #include <boost/algorithm/string.hpp>
+#include <iostream>
 
-REGISTER_PLUGIN(scrimmage::Controller, scrimmage::controller::JSBSimControlControllerHeadingPID, JSBSimControlControllerHeadingPID_plugin)
+REGISTER_PLUGIN(scrimmage::Controller, scrimmage::controller::JSBSimControlControllerHeadingPID,
+                JSBSimControlControllerHeadingPID_plugin)
 
 namespace scrimmage {
 namespace controller {
@@ -48,7 +48,7 @@ namespace controller {
 namespace sc = scrimmage;
 using ang = scrimmage::Angles;
 
-void JSBSimControlControllerHeadingPID::init(std::map<std::string, std::string> &params) {
+void JSBSimControlControllerHeadingPID::init(std::map<std::string, std::string>& params) {
     angles_from_jsbsim_.set_input_clock_direction(ang::Rotate::CW);
     angles_from_jsbsim_.set_input_zero_axis(ang::HeadingZero::Pos_Y);
     angles_from_jsbsim_.set_output_clock_direction(ang::Rotate::CCW);
@@ -77,7 +77,6 @@ void JSBSimControlControllerHeadingPID::init(std::map<std::string, std::string>
 }
 
 bool JSBSimControlControllerHeadingPID::step(double t, double dt) {
-
     // Roll stabilizer
     roll_pid_.set_setpoint(0);
     double u_roll = roll_pid_.step(dt, state_->quat().roll());
@@ -95,5 +94,5 @@ bool JSBSimControlControllerHeadingPID::step(double t, double dt) {
 
     return true;
 }
-} // namespace controller
-} // namespace scrimmage
+}  // namespace controller
+}  // namespace scrimmage
diff --git a/src/plugins/controller/JSBSimModelControllerDirect/JSBSimModelControllerDirect.cpp b/src/plugins/controller/JSBSimModelControllerDirect/JSBSimModelControllerDirect.cpp
index 8fcbd3d562..867db92b1a 100644
--- a/src/plugins/controller/JSBSimModelControllerDirect/JSBSimModelControllerDirect.cpp
+++ b/src/plugins/controller/JSBSimModelControllerDirect/JSBSimModelControllerDirect.cpp
@@ -36,16 +36,16 @@
 
 #include <boost/algorithm/clamp.hpp>
 
-REGISTER_PLUGIN(scrimmage::Controller, scrimmage::controller::JSBSimModelControllerDirect, JSBSimModelControllerDirect_plugin)
+REGISTER_PLUGIN(scrimmage::Controller, scrimmage::controller::JSBSimModelControllerDirect,
+                JSBSimModelControllerDirect_plugin)
 
 namespace scrimmage {
 namespace controller {
 
-void JSBSimModelControllerDirect::init(std::map<std::string, std::string> &params) {
+void JSBSimModelControllerDirect::init(std::map<std::string, std::string>& params) {
     use_pitch_ = str2bool(params.at("use_pitch"));
-    std::string z_name =  use_pitch_ ?
-        vars_.type_map().at(VariableIO::Type::desired_pitch) :
-        vars_.type_map().at(VariableIO::Type::desired_altitude);
+    std::string z_name = use_pitch_ ? vars_.type_map().at(VariableIO::Type::desired_pitch)
+                                    : vars_.type_map().at(VariableIO::Type::desired_altitude);
 
     input_vel_idx_ = vars_.declare(VariableIO::Type::desired_speed, VariableIO::Direction::In);
     input_roll_idx_ = vars_.declare(VariableIO::Type::desired_roll, VariableIO::Direction::In);
@@ -62,5 +62,5 @@ bool JSBSimModelControllerDirect::step(double t, double dt) {
     vars_.output(input_alt_or_pitch_idx_, vars_.input(input_alt_or_pitch_idx_));
     return true;
 }
-} // namespace controller
-} // namespace scrimmage
+}  // namespace controller
+}  // namespace scrimmage
diff --git a/src/plugins/controller/JSBSimModelControllerHeadingPID/JSBSimModelControllerHeadingPID.cpp b/src/plugins/controller/JSBSimModelControllerHeadingPID/JSBSimModelControllerHeadingPID.cpp
index 5a2077a4c8..62839bddeb 100644
--- a/src/plugins/controller/JSBSimModelControllerHeadingPID/JSBSimModelControllerHeadingPID.cpp
+++ b/src/plugins/controller/JSBSimModelControllerHeadingPID/JSBSimModelControllerHeadingPID.cpp
@@ -37,14 +37,15 @@
 
 #include <boost/algorithm/clamp.hpp>
 
-REGISTER_PLUGIN(scrimmage::Controller, scrimmage::controller::JSBSimModelControllerHeadingPID, JSBSimModelControllerHeadingPID_plugin)
+REGISTER_PLUGIN(scrimmage::Controller, scrimmage::controller::JSBSimModelControllerHeadingPID,
+                JSBSimModelControllerHeadingPID_plugin)
 
 namespace scrimmage {
 namespace controller {
 
 using ang = scrimmage::Angles;
 
-void JSBSimModelControllerHeadingPID::init(std::map<std::string, std::string> &params) {
+void JSBSimModelControllerHeadingPID::init(std::map<std::string, std::string>& params) {
     angles_from_jsbsim_.set_input_clock_direction(ang::Rotate::CW);
     angles_from_jsbsim_.set_input_zero_axis(ang::HeadingZero::Pos_Y);
     angles_from_jsbsim_.set_output_clock_direction(ang::Rotate::CCW);
@@ -63,16 +64,17 @@ void JSBSimModelControllerHeadingPID::init(std::map<std::string, std::string> &p
     max_bank_ = ang::deg2rad(std::stod(params.at("max_bank")));
 
     input_vel_idx_ = vars_.declare(VariableIO::Type::desired_speed, VariableIO::Direction::In);
-    input_heading_idx_ = vars_.declare(VariableIO::Type::desired_heading, VariableIO::Direction::In);
+    input_heading_idx_ =
+        vars_.declare(VariableIO::Type::desired_heading, VariableIO::Direction::In);
     input_alt_idx_ = vars_.declare(VariableIO::Type::desired_altitude, VariableIO::Direction::In);
 
     output_vel_idx_ = vars_.declare(VariableIO::Type::desired_speed, VariableIO::Direction::Out);
     output_bank_idx_ = vars_.declare(VariableIO::Type::desired_roll, VariableIO::Direction::Out);
 
     // Is the motion model using pitch or altitude control
-    std::string z_name = vars_.exists(VariableIO::Type::desired_pitch, VariableIO::Direction::Out) ?
-        vars_.type_map().at(VariableIO::Type::desired_pitch) :
-        vars_.type_map().at(VariableIO::Type::desired_altitude);
+    std::string z_name = vars_.exists(VariableIO::Type::desired_pitch, VariableIO::Direction::Out)
+                             ? vars_.type_map().at(VariableIO::Type::desired_pitch)
+                             : vars_.type_map().at(VariableIO::Type::desired_altitude);
 
     output_alt_idx_ = vars_.declare(z_name, VariableIO::Direction::Out);
 }
@@ -92,7 +94,7 @@ bool JSBSimModelControllerHeadingPID::step(double t, double dt) {
     // Might have to use the lag
     double desired_yaw_lag;
     if (heading_lag_initialized_) {
-        double k = 1.00; // disable lag
+        double k = 1.00;  // disable lag
         if (ang::angle_pi(std::abs(desired_yaw - yaw)) > 2.35619) {
             k = dt * 0.75;
         }
@@ -120,5 +122,5 @@ bool JSBSimModelControllerHeadingPID::step(double t, double dt) {
     return true;
 }
 
-} // namespace controller
-} // namespace scrimmage
+}  // namespace controller
+}  // namespace scrimmage
diff --git a/src/plugins/controller/JoystickController/Joystick.cpp b/src/plugins/controller/JoystickController/Joystick.cpp
index b9739214ea..f8fb127408 100644
--- a/src/plugins/controller/JoystickController/Joystick.cpp
+++ b/src/plugins/controller/JoystickController/Joystick.cpp
@@ -30,12 +30,12 @@
  *
  */
 
+#include <scrimmage/entity/EntityPlugin.h>
 #include <scrimmage/parse/ParseUtils.h>
 #include <scrimmage/plugins/controller/JoystickController/Joystick.h>
-#include <scrimmage/entity/EntityPlugin.h>
 
-#include <iostream>
 #include <algorithm>
+#include <iostream>
 
 using std::cout;
 using std::endl;
@@ -53,23 +53,23 @@ Joystick::~Joystick() {
     free(button_);
 }
 
-void Joystick::init(std::map<std::string, std::string> &params,
-                    VariableIO &vars, EntityPluginPtr plugin) {
+void Joystick::init(std::map<std::string, std::string>& params, VariableIO& vars,
+                    EntityPluginPtr plugin) {
     print_js_values_ = sc::get<bool>("print_raw_joystick_values", params, false);
 
     std::string dev = sc::get<std::string>("device", params, "/dev/input/js0");
     if ((joy_fd_ = open(dev.c_str(), O_RDONLY)) == -1) {
-		cout << "couldn't open joystick: " << dev << endl;
-	}
+        cout << "couldn't open joystick: " << dev << endl;
+    }
 
     char name_of_joystick[80];
 
     ioctl(joy_fd_, JSIOCGAXES, &num_of_axis_);
-	ioctl(joy_fd_, JSIOCGBUTTONS, &num_of_buttons_);
-	ioctl(joy_fd_, JSIOCGNAME(80), &name_of_joystick);
+    ioctl(joy_fd_, JSIOCGBUTTONS, &num_of_buttons_);
+    ioctl(joy_fd_, JSIOCGNAME(80), &name_of_joystick);
 
-	axis_ = reinterpret_cast<int*>(calloc(num_of_axis_, sizeof(int)));
-	button_ = reinterpret_cast<char*>(calloc(num_of_buttons_, sizeof(char)));
+    axis_ = reinterpret_cast<int*>(calloc(num_of_axis_, sizeof(int)));
+    button_ = reinterpret_cast<char*>(calloc(num_of_buttons_, sizeof(char)));
 
     prev_button_state_.resize(num_of_buttons_);
 
@@ -79,7 +79,7 @@ void Joystick::init(std::map<std::string, std::string> &params,
         cout << "\t " << num_of_buttons_ << " buttons" << endl;
     }
 
-	fcntl(joy_fd_, F_SETFL, O_NONBLOCK); // use non-blocking mode
+    fcntl(joy_fd_, F_SETFL, O_NONBLOCK);  // use non-blocking mode
 
     std::string axis_map = sc::get<std::string>("axis_map", params, "");
     std::vector<std::vector<std::string>> vecs;
@@ -97,11 +97,9 @@ void Joystick::init(std::map<std::string, std::string> &params,
 
             int axis = std::stod(vec[1]);
             if (axis >= num_of_axis_) {
-                cout << "Warning: axis_map contains out-of-range axis index"
-                     << endl;
+                cout << "Warning: axis_map contains out-of-range axis index" << endl;
             } else {
-                AxisScale at(axis, std::stod(vec[2]),
-                             std::stod(vec[3]), std::stod(vec[4]),
+                AxisScale at(axis, std::stod(vec[2]), std::stod(vec[3]), std::stod(vec[4]),
                              std::stod(vec[5]), std::stod(vec[6]),
                              vars.declare(vec[0], VariableIO::Direction::Out));
                 axis_tfs_.push_back(at);
@@ -111,13 +109,14 @@ void Joystick::init(std::map<std::string, std::string> &params,
 
     publish_button_state_ = sc::get<bool>("publish_button_state", params, false);
     std::string button_topic = sc::get<std::string>("button_topic", params, "joystick_buttons");
-    std::string button_network_name = sc::get<std::string>("button_network_name", params, "LocalNetwork");
+    std::string button_network_name =
+        sc::get<std::string>("button_network_name", params, "LocalNetwork");
     if (publish_button_state_) {
         pub_buttons_ = plugin->advertise(button_network_name, button_topic);
     }
 }
 
-bool Joystick::step(double t, double dt, VariableIO &vars) {
+bool Joystick::step(double t, double dt, VariableIO& vars) {
     int bytes = read(joy_fd_, &js_, sizeof(struct js_event));
     if (bytes == -1) {
         // nop, avoid unused variable warning
@@ -126,18 +125,18 @@ bool Joystick::step(double t, double dt, VariableIO &vars) {
     // see what to do with the event
     bool button_changed = false;
     switch (js_.type & ~JS_EVENT_INIT) {
-    case JS_EVENT_AXIS:
-        axis_[js_.number] = js_.value;
-        break;
-    case JS_EVENT_BUTTON:
-        button_[js_.number] = js_.value;
-        button_changed = true;
-        break;
+        case JS_EVENT_AXIS:
+            axis_[js_.number] = js_.value;
+            break;
+        case JS_EVENT_BUTTON:
+            button_[js_.number] = js_.value;
+            button_changed = true;
+            break;
     }
 
     if (print_js_values_) {
         for (int x = 0; x < num_of_axis_; x++) {
-            printf("%d: %6d  ", x, axis_[x] );
+            printf("%d: %6d  ", x, axis_[x]);
         }
 
         for (int x = 0; x < num_of_buttons_; x++) {
@@ -148,8 +147,7 @@ bool Joystick::step(double t, double dt, VariableIO &vars) {
     }
 
     for (AxisScale axis_tf : axis_tfs_) {
-        vars.output(axis_tf.vector_index(),
-                    axis_tf.scale(axis_[axis_tf.axis_index()]));
+        vars.output(axis_tf.vector_index(), axis_tf.scale(axis_[axis_tf.axis_index()]));
     }
 
     if (publish_button_state_) {
@@ -162,8 +160,7 @@ bool Joystick::step(double t, double dt, VariableIO &vars) {
                 msg->data[i] = button_[i];
             }
 
-            auto it = std::mismatch(msg->data.begin(), msg->data.end(),
-                              prev_button_state_.begin());
+            auto it = std::mismatch(msg->data.begin(), msg->data.end(), prev_button_state_.begin());
             if (it.first != msg->data.end()) {
                 pub_buttons_->publish(msg);
                 prev_button_state_ = msg->data;
@@ -174,5 +171,5 @@ bool Joystick::step(double t, double dt, VariableIO &vars) {
     return true;
 }
 
-} // namespace controller
-} // namespace scrimmage
+}  // namespace controller
+}  // namespace scrimmage
diff --git a/src/plugins/controller/JoystickController/JoystickController.cpp b/src/plugins/controller/JoystickController/JoystickController.cpp
index 149654bd12..8b833bd1f9 100644
--- a/src/plugins/controller/JoystickController/JoystickController.cpp
+++ b/src/plugins/controller/JoystickController/JoystickController.cpp
@@ -30,13 +30,12 @@
  *
  */
 
-#include <scrimmage/plugins/controller/JoystickController/JoystickController.h>
-
-#include <scrimmage/plugin_manager/RegisterPlugin.h>
+#include <scrimmage/common/Utilities.h>
 #include <scrimmage/entity/Entity.h>
 #include <scrimmage/math/State.h>
-#include <scrimmage/common/Utilities.h>
 #include <scrimmage/parse/ParseUtils.h>
+#include <scrimmage/plugin_manager/RegisterPlugin.h>
+#include <scrimmage/plugins/controller/JoystickController/JoystickController.h>
 #include <scrimmage/pubsub/PubSub.h>
 
 #include <iostream>
@@ -47,20 +46,18 @@ using std::endl;
 
 namespace sc = scrimmage;
 
-REGISTER_PLUGIN(scrimmage::Controller,
-                scrimmage::controller::JoystickController,
+REGISTER_PLUGIN(scrimmage::Controller, scrimmage::controller::JoystickController,
                 JoystickController_plugin)
 
 namespace scrimmage {
 namespace controller {
 
-void JoystickController::init(std::map<std::string, std::string> &params) {
-    joystick_.init(params, vars_,
-                   std::static_pointer_cast<EntityPlugin>(shared_from_this()));
+void JoystickController::init(std::map<std::string, std::string>& params) {
+    joystick_.init(params, vars_, std::static_pointer_cast<EntityPlugin>(shared_from_this()));
 }
 
 bool JoystickController::step(double t, double dt) {
     return joystick_.step(t, dt, vars_);
 }
-} // namespace controller
-} // namespace scrimmage
+}  // namespace controller
+}  // namespace scrimmage
diff --git a/src/plugins/controller/MotionBattery/MotionBattery.cpp b/src/plugins/controller/MotionBattery/MotionBattery.cpp
index 91be912c58..10b6141396 100644
--- a/src/plugins/controller/MotionBattery/MotionBattery.cpp
+++ b/src/plugins/controller/MotionBattery/MotionBattery.cpp
@@ -30,15 +30,14 @@
  *
  */
 
-#include <scrimmage/plugins/controller/MotionBattery/MotionBattery.h>
-
-#include <scrimmage/plugin_manager/RegisterPlugin.h>
+#include <scrimmage/common/Time.h>
+#include <scrimmage/common/Utilities.h>
 #include <scrimmage/entity/Entity.h>
 #include <scrimmage/math/State.h>
-#include <scrimmage/common/Utilities.h>
-#include <scrimmage/common/Time.h>
-#include <scrimmage/parse/ParseUtils.h>
 #include <scrimmage/msgs/Battery.pb.h>
+#include <scrimmage/parse/ParseUtils.h>
+#include <scrimmage/plugin_manager/RegisterPlugin.h>
+#include <scrimmage/plugins/controller/MotionBattery/MotionBattery.h>
 #include <scrimmage/pubsub/Publisher.h>
 
 #include <iostream>
@@ -51,17 +50,15 @@ namespace sc = scrimmage;
 namespace sm = scrimmage_msgs;
 namespace pl = std::placeholders;
 
-REGISTER_PLUGIN(scrimmage::Controller,
-                scrimmage::controller::MotionBattery,
-                MotionBattery_plugin)
+REGISTER_PLUGIN(scrimmage::Controller, scrimmage::controller::MotionBattery, MotionBattery_plugin)
 
 namespace scrimmage {
 namespace controller {
 
-void MotionBattery::init(std::map<std::string, std::string> &params) {
+void MotionBattery::init(std::map<std::string, std::string>& params) {
     // Discover the output variables that point to the input variables for the
     // motion model. Setup input variables that mirror the output variables.
-    for (auto &kv : vars_.output_variable_index()) {
+    for (auto& kv : vars_.output_variable_index()) {
         int out_idx = vars_.declare(kv.first, VariableIO::Direction::Out);
         int in_idx = vars_.declare(kv.first, VariableIO::Direction::In);
         io_map_[kv.first] = std::make_unique<VarLimit>(in_idx, out_idx, 0.0, 0.0, false);
@@ -73,9 +70,8 @@ void MotionBattery::init(std::map<std::string, std::string> &params) {
     battery_ = Battery(charge_min, charge_max, charge);
 
     std::vector<std::vector<std::string>> vecs;
-    if (get_vec_of_vecs(sc::get<std::string>("depletion_map", params, ""),
-                        vecs)) {
-        for (auto &vec : vecs) {
+    if (get_vec_of_vecs(sc::get<std::string>("depletion_map", params, ""), vecs)) {
+        for (auto& vec : vecs) {
             if (vec.size() != 5) {
                 cout << "Invalid depletion mapping: " << endl;
                 for (std::string s : vec) {
@@ -99,7 +95,7 @@ void MotionBattery::init(std::map<std::string, std::string> &params) {
     }
 
     // Charging subscriber
-    auto callback_charge_added = [&] (scrimmage::MessagePtr<sm::Charge> msg) {
+    auto callback_charge_added = [&](scrimmage::MessagePtr<sm::Charge> msg) {
         if (parent_->id().id() == msg->data.id()) {
             battery_.add_charge(msg->data.charge_amount());
         }
@@ -122,7 +118,7 @@ bool MotionBattery::step(double t, double dt) {
 
     // Loop over all variables in the io_map_, apply charge calculation, and
     // limit output if required
-    for (auto &kv : io_map_) {
+    for (auto& kv : io_map_) {
         double value = vars_.input(kv.second->input_idx);
         double depletion = kv.second->calc_depletion(value, time_->dt());
         if (!battery_.deplete(depletion)) {
@@ -139,15 +135,14 @@ bool MotionBattery::step(double t, double dt) {
 }
 
 bool MotionBattery::get_battery_charge(scrimmage::MessageBasePtr request,
-                                       scrimmage::MessageBasePtr &response) {
-    response =
-        std::make_shared<sc::Message<double>>(battery_.current_charge());
+                                       scrimmage::MessageBasePtr& response) {
+    response = std::make_shared<sc::Message<double>>(battery_.current_charge());
     return true;
 }
 
-double MotionBattery::calculate_charge_usage(const double &throttle, const double &dt) {
+double MotionBattery::calculate_charge_usage(const double& throttle, const double& dt) {
     return 0.0;
 }
 
-} // namespace controller
-} // namespace scrimmage
+}  // namespace controller
+}  // namespace scrimmage
diff --git a/src/plugins/controller/MultirotorControllerOmega/MultirotorControllerOmega.cpp b/src/plugins/controller/MultirotorControllerOmega/MultirotorControllerOmega.cpp
index 78d3277c4e..a0b20e92ad 100644
--- a/src/plugins/controller/MultirotorControllerOmega/MultirotorControllerOmega.cpp
+++ b/src/plugins/controller/MultirotorControllerOmega/MultirotorControllerOmega.cpp
@@ -30,14 +30,14 @@
  *
  */
 
-#include <scrimmage/plugin_manager/RegisterPlugin.h>
+#include <scrimmage/common/Utilities.h>
 #include <scrimmage/entity/Entity.h>
 #include <scrimmage/math/State.h>
-#include <scrimmage/common/Utilities.h>
 #include <scrimmage/parse/ParseUtils.h>
+#include <scrimmage/plugin_manager/RegisterPlugin.h>
+#include <scrimmage/plugins/autonomy/ArduPilot/PwmState.h>
 #include <scrimmage/plugins/controller/MultirotorControllerOmega/MultirotorControllerOmega.h>
 #include <scrimmage/plugins/motion/Multirotor/MultirotorState.h>
-#include <scrimmage/plugins/autonomy/ArduPilot/PwmState.h>
 
 #include <iostream>
 #include <limits>
@@ -49,19 +49,16 @@ using std::endl;
 namespace sc = scrimmage;
 namespace sm = scrimmage::motion;
 
-REGISTER_PLUGIN(scrimmage::Controller,
-                scrimmage::controller::MultirotorControllerOmega,
+REGISTER_PLUGIN(scrimmage::Controller, scrimmage::controller::MultirotorControllerOmega,
                 MultirotorControllerOmega_plugin)
 
 namespace scrimmage {
 namespace controller {
 
-MultirotorControllerOmega::MultirotorControllerOmega() : pwm_max_(2000),
-                                                         pwm_min_(1000) {
+MultirotorControllerOmega::MultirotorControllerOmega() : pwm_max_(2000), pwm_min_(1000) {
 }
 
-void MultirotorControllerOmega::init(std::map<std::string, std::string> &params) {
-
+void MultirotorControllerOmega::init(std::map<std::string, std::string>& params) {
     pwm_max_ = sc::get<int>("pwm_max", params, 2000);
     pwm_min_ = sc::get<int>("pwm_min", params, 1000);
 
@@ -84,21 +81,19 @@ bool MultirotorControllerOmega::step(double t, double dt) {
         if (d_state->input_type() == sm::MultirotorState::InputType::OMEGA) {
             u_ = d_state->prop_input().head(multirotor_->rotors().size());
         } else if (d_state->input_type() == sm::MultirotorState::InputType::PWM) {
-            u_ = sc::scale(d_state->prop_input().head(multirotor_->rotors().size()), pwm_min_, pwm_max_,
-                           multirotor_->omega_min(),
-                           multirotor_->omega_max());
+            u_ = sc::scale(d_state->prop_input().head(multirotor_->rotors().size()), pwm_min_,
+                           pwm_max_, multirotor_->omega_min(), multirotor_->omega_max());
         } else {
             cout << "WARNING: Invalid MultirotorState input type" << endl;
         }
-    } else if ( d_state2 ) {
+    } else if (d_state2) {
         u_ = sc::scale(d_state2->pwm_input().head(multirotor_->rotors().size()), pwm_min_, pwm_max_,
-                       multirotor_->omega_min(),
-                       multirotor_->omega_max());
+                       multirotor_->omega_min(), multirotor_->omega_max());
     } else {
         cout << "Unable to cast desired_state to MultirotorState" << endl;
         u_ = Eigen::VectorXd::Zero(multirotor_->rotors().size());
     }
     return true;
 }
-} // namespace controller
-} // namespace scrimmage
+}  // namespace controller
+}  // namespace scrimmage
diff --git a/src/plugins/controller/MultirotorControllerPID/MultirotorControllerPID.cpp b/src/plugins/controller/MultirotorControllerPID/MultirotorControllerPID.cpp
index ab194d7d82..f73c731b82 100644
--- a/src/plugins/controller/MultirotorControllerPID/MultirotorControllerPID.cpp
+++ b/src/plugins/controller/MultirotorControllerPID/MultirotorControllerPID.cpp
@@ -30,14 +30,13 @@
  *
  */
 
-#include <scrimmage/plugins/controller/MultirotorControllerPID/MultirotorControllerPID.h>
-#include <scrimmage/plugins/controller/MultirotorControllerOmega/MultirotorControllerOmega.h>
-#include <scrimmage/plugins/motion/Multirotor/MultirotorState.h>
-
-#include <scrimmage/plugin_manager/RegisterPlugin.h>
 #include <scrimmage/entity/Entity.h>
 #include <scrimmage/math/State.h>
 #include <scrimmage/parse/ParseUtils.h>
+#include <scrimmage/plugin_manager/RegisterPlugin.h>
+#include <scrimmage/plugins/controller/MultirotorControllerOmega/MultirotorControllerOmega.h>
+#include <scrimmage/plugins/controller/MultirotorControllerPID/MultirotorControllerPID.h>
+#include <scrimmage/plugins/motion/Multirotor/MultirotorState.h>
 
 #include <iostream>
 #include <limits>
@@ -47,8 +46,7 @@ using std::endl;
 
 namespace sc = scrimmage;
 
-REGISTER_PLUGIN(scrimmage::Controller,
-                scrimmage::controller::MultirotorControllerPID,
+REGISTER_PLUGIN(scrimmage::Controller, scrimmage::controller::MultirotorControllerPID,
                 MultirotorControllerPID_plugin)
 
 namespace scrimmage {
@@ -57,7 +55,7 @@ namespace controller {
 MultirotorControllerPID::MultirotorControllerPID() {
 }
 
-void MultirotorControllerPID::init(std::map<std::string, std::string> &params) {
+void MultirotorControllerPID::init(std::map<std::string, std::string>& params) {
     multirotor_ = std::dynamic_pointer_cast<sc::motion::Multirotor>(parent_->motion());
 
     if (multirotor_ == nullptr) {
@@ -77,22 +75,19 @@ void MultirotorControllerPID::init(std::map<std::string, std::string> &params) {
     for (int i = 0; i < 3; i++) {
         vel_pids_.push_back(sc::PID());
         if (!sc::set_pid_gains(vel_pids_[i], params["vel_pid"])) {
-            cout << "Failed to set DoubleIntegratorControllerVewYaw vel gain"
-                 << endl;
+            cout << "Failed to set DoubleIntegratorControllerVewYaw vel gain" << endl;
         }
     }
 
     for (int i = 0; i < 3; i++) {
         angle_pids_.push_back(sc::PID());
         if (!sc::set_pid_gains(angle_pids_[i], params["angle_pid"])) {
-            cout << "Failed to set DoubleIntegratorControllerVewYaw angle PID gains."
-                 << endl;
+            cout << "Failed to set DoubleIntegratorControllerVewYaw angle PID gains." << endl;
         }
     }
 }
 
 bool MultirotorControllerPID::step(double t, double dt) {
-
     Eigen::Vector3d des_vel_body = state_->quat().rotate_reverse(desired_state_->vel());
     Eigen::Vector3d vel_body = state_->quat().rotate_reverse(state_->vel());
     Eigen::Vector3d vel_ctrl(0, 0, 0);
@@ -105,8 +100,8 @@ bool MultirotorControllerPID::step(double t, double dt) {
     // cout << "Desired Velocity (Body): " << des_vel_body << endl;
 
     for (unsigned int i = 0; i < multirotor_->rotors().size(); i++) {
-        u_(i) = sqrt(multirotor_->mass() * multirotor_->gravity_magnitude() /
-                     (multirotor_->rotors().size() * multirotor_->c_T()));
+        u_(i) = sqrt(multirotor_->mass() * multirotor_->gravity_magnitude()
+                     / (multirotor_->rotors().size() * multirotor_->c_T()));
     }
 
     // double desired_roll = 0;
@@ -127,11 +122,10 @@ bool MultirotorControllerPID::step(double t, double dt) {
     // u_(0) -= pitch_ctrl * 1;
     // u_(3) -= pitch_ctrl * 1;
 
-
     // yaw_pid_.set_setpoint(desired_state_->quat().yaw());
     // double yaw_ctrl = yaw_pid_.step(dt, state_->quat().yaw());
 
     return true;
 }
-} // namespace controller
-} // namespace scrimmage
+}  // namespace controller
+}  // namespace scrimmage
diff --git a/src/plugins/controller/RigidBody6DOFControllerPID/RigidBody6DOFControllerPID.cpp b/src/plugins/controller/RigidBody6DOFControllerPID/RigidBody6DOFControllerPID.cpp
index 39e5be80d7..1dedb2cb16 100644
--- a/src/plugins/controller/RigidBody6DOFControllerPID/RigidBody6DOFControllerPID.cpp
+++ b/src/plugins/controller/RigidBody6DOFControllerPID/RigidBody6DOFControllerPID.cpp
@@ -34,18 +34,18 @@
 #include <scrimmage/plugin_manager/RegisterPlugin.h>
 #include <scrimmage/plugins/controller/RigidBody6DOFControllerPID/RigidBody6DOFControllerPID.h>
 
-#include <iostream>
-
 #include <boost/algorithm/string.hpp>
+#include <iostream>
 
-REGISTER_PLUGIN(scrimmage::Controller, scrimmage::controller::RigidBody6DOFControllerPID, RigidBody6DOFControllerPID_plugin)
+REGISTER_PLUGIN(scrimmage::Controller, scrimmage::controller::RigidBody6DOFControllerPID,
+                RigidBody6DOFControllerPID_plugin)
 
 namespace scrimmage {
 namespace controller {
 
 namespace sc = scrimmage;
 
-void set_pid(sc::PID &pid, std::string str, bool is_angle) {
+void set_pid(sc::PID& pid, std::string str, bool is_angle) {
     std::vector<std::string> str_vals;
     boost::split(str_vals, str, boost::is_any_of(","));
 
@@ -68,7 +68,7 @@ void set_pid(sc::PID &pid, std::string str, bool is_angle) {
     }
 }
 
-void RigidBody6DOFControllerPID::init(std::map<std::string, std::string> &params) {
+void RigidBody6DOFControllerPID::init(std::map<std::string, std::string>& params) {
     set_pid(heading_pid_, params["heading_pid"], true);
     set_pid(alt_pid_, params["alt_pid"], false);
     set_pid(vel_pid_, params["vel_pid"], false);
@@ -92,5 +92,5 @@ bool RigidBody6DOFControllerPID::step(double t, double dt) {
     (*u_) << u_thrust, roll_error, pitch_error, 0;
     return true;
 }
-} // namespace controller
-} // namespace scrimmage
+}  // namespace controller
+}  // namespace scrimmage
diff --git a/src/plugins/controller/RigidBody6DOFControllerROS/RigidBody6DOFControllerROS.cpp b/src/plugins/controller/RigidBody6DOFControllerROS/RigidBody6DOFControllerROS.cpp
index 33d8a16bb7..8960b87f4a 100644
--- a/src/plugins/controller/RigidBody6DOFControllerROS/RigidBody6DOFControllerROS.cpp
+++ b/src/plugins/controller/RigidBody6DOFControllerROS/RigidBody6DOFControllerROS.cpp
@@ -32,16 +32,18 @@
 
 #include <scrimmage/plugin_manager/RegisterPlugin.h>
 #include <scrimmage/plugins/controller/RigidBody6DOFControllerROS/RigidBody6DOFControllerROS.h>
+
 #include <boost/algorithm/string.hpp>
 
-REGISTER_PLUGIN(scrimmage::Controller, scrimmage::controller::RigidBody6DOFControllerROS, RigidBody6DOFControllerROS_plugin)
+REGISTER_PLUGIN(scrimmage::Controller, scrimmage::controller::RigidBody6DOFControllerROS,
+                RigidBody6DOFControllerROS_plugin)
 
 namespace scrimmage {
 namespace controller {
 
 namespace sc = scrimmage;
 
-void RigidBody6DOFControllerROS::init(std::map<std::string, std::string> &params) {
+void RigidBody6DOFControllerROS::init(std::map<std::string, std::string>& params) {
     u_ = std::make_shared<Eigen::Vector4d>();
 
     if (!ros::isInitialized()) {
@@ -72,5 +74,5 @@ bool RigidBody6DOFControllerROS::step(double t, double dt) {
 void RigidBody6DOFControllerROS::cmd_vel_cb(const geometry_msgs::Twist::ConstPtr& msg) {
     cmd_vel_ = *msg;
 }
-} // namespace controller
-} // namespace scrimmage
+}  // namespace controller
+}  // namespace scrimmage
diff --git a/src/plugins/controller/SimpleAircraftControllerPID/SimpleAircraftControllerPID.cpp b/src/plugins/controller/SimpleAircraftControllerPID/SimpleAircraftControllerPID.cpp
index ef20409d98..e7cbac7bf8 100644
--- a/src/plugins/controller/SimpleAircraftControllerPID/SimpleAircraftControllerPID.cpp
+++ b/src/plugins/controller/SimpleAircraftControllerPID/SimpleAircraftControllerPID.cpp
@@ -39,14 +39,15 @@
 
 #include <boost/algorithm/string.hpp>
 
-REGISTER_PLUGIN(scrimmage::Controller, scrimmage::controller::SimpleAircraftControllerPID, SimpleAircraftControllerPID_plugin)
+REGISTER_PLUGIN(scrimmage::Controller, scrimmage::controller::SimpleAircraftControllerPID,
+                SimpleAircraftControllerPID_plugin)
 
 namespace scrimmage {
 namespace controller {
 
 namespace sc = scrimmage;
 
-void SimpleAircraftControllerPID::init(std::map<std::string, std::string> &params) {
+void SimpleAircraftControllerPID::init(std::map<std::string, std::string>& params) {
     if (!heading_pid_.init(params["heading_pid"], true)) {
         std::cout << "Failed to set heading PID" << std::endl;
     }
@@ -60,13 +61,12 @@ void SimpleAircraftControllerPID::init(std::map<std::string, std::string> &param
     use_glide_slope_ = sc::str2bool(params.at("use_glide_slope"));
 
     // Set up to use either roll or heading
-    std::string ctrl_name = use_roll_ ?
-        vars_.type_map().at(VariableIO::Type::desired_roll) :
-        vars_.type_map().at(VariableIO::Type::desired_heading);
+    std::string ctrl_name = use_roll_ ? vars_.type_map().at(VariableIO::Type::desired_roll)
+                                      : vars_.type_map().at(VariableIO::Type::desired_heading);
     // Set up to use either altitude or glide slope (z_vel / sqrt(x_vel^2 + y_vel^2))
-    std::string alt_ctrl_name = use_glide_slope_ ?
-        vars_.type_map().at(VariableIO::Type::desired_glide_slope) :
-        vars_.type_map().at(VariableIO::Type::desired_altitude);
+    std::string alt_ctrl_name = use_glide_slope_
+                                    ? vars_.type_map().at(VariableIO::Type::desired_glide_slope)
+                                    : vars_.type_map().at(VariableIO::Type::desired_altitude);
 
     input_roll_or_heading_idx_ = vars_.declare(ctrl_name, VariableIO::Direction::In);
     input_altitude_or_glide_slope_idx_ = vars_.declare(alt_ctrl_name, VariableIO::Direction::In);
@@ -74,19 +74,21 @@ void SimpleAircraftControllerPID::init(std::map<std::string, std::string> &param
 
     output_throttle_idx_ = vars_.declare(VariableIO::Type::throttle, VariableIO::Direction::Out);
     output_roll_rate_idx_ = vars_.declare(VariableIO::Type::roll_rate, VariableIO::Direction::Out);
-    output_pitch_rate_idx_ = vars_.declare(VariableIO::Type::pitch_rate, VariableIO::Direction::Out);
+    output_pitch_rate_idx_ =
+        vars_.declare(VariableIO::Type::pitch_rate, VariableIO::Direction::Out);
 }
 
 bool SimpleAircraftControllerPID::step(double t, double dt) {
     heading_pid_.set_setpoint(vars_.input(input_roll_or_heading_idx_));
-    double u_roll_rate = use_roll_ ?
-        -heading_pid_.step(dt, state_->quat().roll()) :
-        heading_pid_.step(dt, state_->quat().yaw());
+    double u_roll_rate = use_roll_ ? -heading_pid_.step(dt, state_->quat().roll())
+                                   : heading_pid_.step(dt, state_->quat().yaw());
 
     alt_pid_.set_setpoint(vars_.input(input_altitude_or_glide_slope_idx_));
-    double u_pitch_rate = use_glide_slope_ ?
-        alt_pid_.step(dt, atan(state_->vel()(2) / sqrt(state_->vel()(0) * state_->vel()(0) + state_->vel()(1) * state_->vel()(1)))) :
-        -alt_pid_.step(dt, state_->pos()(2));
+    double u_pitch_rate =
+        use_glide_slope_ ? alt_pid_.step(dt, atan(state_->vel()(2)
+                                                  / sqrt(state_->vel()(0) * state_->vel()(0)
+                                                         + state_->vel()(1) * state_->vel()(1))))
+                         : -alt_pid_.step(dt, state_->pos()(2));
 
     vel_pid_.set_setpoint(vars_.input(input_velocity_idx_));
     double u_throttle = vel_pid_.step(dt, state_->vel().norm());
diff --git a/src/plugins/controller/SimpleCarControllerHeading/SimpleCarControllerHeading.cpp b/src/plugins/controller/SimpleCarControllerHeading/SimpleCarControllerHeading.cpp
index 217cafddde..40b332f0dc 100644
--- a/src/plugins/controller/SimpleCarControllerHeading/SimpleCarControllerHeading.cpp
+++ b/src/plugins/controller/SimpleCarControllerHeading/SimpleCarControllerHeading.cpp
@@ -36,12 +36,13 @@
 #include <scrimmage/plugin_manager/RegisterPlugin.h>
 #include <scrimmage/plugins/controller/SimpleCarControllerHeading/SimpleCarControllerHeading.h>
 
-REGISTER_PLUGIN(scrimmage::Controller, scrimmage::controller::SimpleCarControllerHeading, SimpleCarControllerHeading_plugin)
+REGISTER_PLUGIN(scrimmage::Controller, scrimmage::controller::SimpleCarControllerHeading,
+                SimpleCarControllerHeading_plugin)
 
 namespace scrimmage {
 namespace controller {
 
-void SimpleCarControllerHeading::init(std::map<std::string, std::string> &params) {
+void SimpleCarControllerHeading::init(std::map<std::string, std::string>& params) {
     double p_gain = get<double>("p_gain", params, 1);
     double i_gain = get<double>("i_gain", params, 1);
     double d_gain = get<double>("d_gain", params, 1);
@@ -51,7 +52,8 @@ void SimpleCarControllerHeading::init(std::map<std::string, std::string> &params
     pid_.set_is_angle(true);
 
     input_vel_idx_ = vars_.declare(VariableIO::Type::desired_speed, VariableIO::Direction::In);
-    input_heading_idx_ = vars_.declare(VariableIO::Type::desired_heading, VariableIO::Direction::In);
+    input_heading_idx_ =
+        vars_.declare(VariableIO::Type::desired_heading, VariableIO::Direction::In);
 
     output_vel_idx_ = vars_.declare(VariableIO::Type::speed, VariableIO::Direction::Out);
     output_turn_rate_idx_ = vars_.declare(VariableIO::Type::turn_rate, VariableIO::Direction::Out);
@@ -65,5 +67,5 @@ bool SimpleCarControllerHeading::step(double t, double dt) {
 
     return true;
 }
-} // namespace controller
-} // namespace scrimmage
+}  // namespace controller
+}  // namespace scrimmage
diff --git a/src/plugins/controller/SimpleQuadrotorControllerLQR/SimpleQuadrotorControllerLQR.cpp b/src/plugins/controller/SimpleQuadrotorControllerLQR/SimpleQuadrotorControllerLQR.cpp
index 099c5ec0f9..12553f90ea 100644
--- a/src/plugins/controller/SimpleQuadrotorControllerLQR/SimpleQuadrotorControllerLQR.cpp
+++ b/src/plugins/controller/SimpleQuadrotorControllerLQR/SimpleQuadrotorControllerLQR.cpp
@@ -38,14 +38,15 @@
 
 #include <boost/algorithm/clamp.hpp>
 
-REGISTER_PLUGIN(scrimmage::Controller, scrimmage::controller::SimpleQuadrotorControllerLQR, SimpleQuadrotorControllerLQR_plugin)
+REGISTER_PLUGIN(scrimmage::Controller, scrimmage::controller::SimpleQuadrotorControllerLQR,
+                SimpleQuadrotorControllerLQR_plugin)
 
 namespace scrimmage {
 namespace controller {
 
 namespace sc = scrimmage;
 
-void SimpleQuadrotorControllerLQR::init(std::map<std::string, std::string> &params) {
+void SimpleQuadrotorControllerLQR::init(std::map<std::string, std::string>& params) {
     double p_gain = sc::get("vel_p_gain", params, 1.0);
     double i_gain = sc::get("vel_i_gain", params, 1.0);
     double d_gain = sc::get("vel_d_gain", params, 1.0);
@@ -57,25 +58,24 @@ void SimpleQuadrotorControllerLQR::init(std::map<std::string, std::string> &para
 }
 
 bool SimpleQuadrotorControllerLQR::step(double t, double dt) {
-    Eigen::Vector3d &des_pos = desired_state_->pos();
+    Eigen::Vector3d& des_pos = desired_state_->pos();
     double des_yaw = desired_state_->quat().yaw();
 
-    Eigen::Vector3d &pos = state_->pos();
-    Eigen::Vector3d &vel = state_->vel();
+    Eigen::Vector3d& pos = state_->pos();
+    Eigen::Vector3d& vel = state_->vel();
     double yaw = state_->quat().yaw();
     double xy_speed = vel.head<2>().norm();
 
-    double yaw_dot =
-        std::isnan(prev_yaw_) ? 0 : sc::Angles::angle_diff_rad(yaw, prev_yaw_) / dt;
+    double yaw_dot = std::isnan(prev_yaw_) ? 0 : sc::Angles::angle_diff_rad(yaw, prev_yaw_) / dt;
     prev_yaw_ = yaw;
 
     // LQR Altitude Controller:
     double q1 = 1;
-    double z_thrust = -1.0 / q1 * (pos(2) - des_pos(2)) - sqrt(2.0/q1) * vel(2);
+    double z_thrust = -1.0 / q1 * (pos(2) - des_pos(2)) - sqrt(2.0 / q1) * vel(2);
 
     // LQR Heading Controller:
     double q2 = 1;
-    double turn_force = -1.0 / q2 * sc::Angles::angle_pi(yaw - des_yaw) - sqrt(2.0/q2) * yaw_dot;
+    double turn_force = -1.0 / q2 * sc::Angles::angle_pi(yaw - des_yaw) - sqrt(2.0 / q2) * yaw_dot;
 
     // If not close to x/y position, use forward velocity:
     double dist = (des_pos - pos).head<2>().norm();
@@ -90,5 +90,5 @@ bool SimpleQuadrotorControllerLQR::step(double t, double dt) {
 
     return true;
 }
-} // namespace controller
-} // namespace scrimmage
+}  // namespace controller
+}  // namespace scrimmage
diff --git a/src/plugins/controller/SingleIntegratorControllerSimple/SingleIntegratorControllerSimple.cpp b/src/plugins/controller/SingleIntegratorControllerSimple/SingleIntegratorControllerSimple.cpp
index 2dc611b0e7..5e694b7719 100644
--- a/src/plugins/controller/SingleIntegratorControllerSimple/SingleIntegratorControllerSimple.cpp
+++ b/src/plugins/controller/SingleIntegratorControllerSimple/SingleIntegratorControllerSimple.cpp
@@ -34,12 +34,13 @@
 #include <scrimmage/plugin_manager/RegisterPlugin.h>
 #include <scrimmage/plugins/controller/SingleIntegratorControllerSimple/SingleIntegratorControllerSimple.h>
 
-REGISTER_PLUGIN(scrimmage::Controller, scrimmage::controller::SingleIntegratorControllerSimple, SingleIntegratorControllerSimple_plugin)
+REGISTER_PLUGIN(scrimmage::Controller, scrimmage::controller::SingleIntegratorControllerSimple,
+                SingleIntegratorControllerSimple_plugin)
 
 namespace scrimmage {
 namespace controller {
 
-void SingleIntegratorControllerSimple::init(std::map<std::string, std::string> &params) {
+void SingleIntegratorControllerSimple::init(std::map<std::string, std::string>& params) {
     input_vel_x_idx_ = vars_.declare(VariableIO::Type::velocity_x, VariableIO::Direction::In);
     input_vel_y_idx_ = vars_.declare(VariableIO::Type::velocity_y, VariableIO::Direction::In);
     input_vel_z_idx_ = vars_.declare(VariableIO::Type::velocity_z, VariableIO::Direction::In);
@@ -55,5 +56,5 @@ bool SingleIntegratorControllerSimple::step(double t, double dt) {
     vars_.output(output_vel_z_idx_, vars_.input(input_vel_z_idx_));
     return true;
 }
-} // namespace controller
-} // namespace scrimmage
+}  // namespace controller
+}  // namespace scrimmage
diff --git a/src/plugins/controller/SingleIntegratorControllerWaypoint/SingleIntegratorControllerWaypoint.cpp b/src/plugins/controller/SingleIntegratorControllerWaypoint/SingleIntegratorControllerWaypoint.cpp
index 17dffea45c..bd40c9e8be 100644
--- a/src/plugins/controller/SingleIntegratorControllerWaypoint/SingleIntegratorControllerWaypoint.cpp
+++ b/src/plugins/controller/SingleIntegratorControllerWaypoint/SingleIntegratorControllerWaypoint.cpp
@@ -31,16 +31,17 @@
  */
 
 #include <scrimmage/common/VariableIO.h>
+#include <scrimmage/math/State.h>
 #include <scrimmage/plugin_manager/RegisterPlugin.h>
 #include <scrimmage/plugins/controller/SingleIntegratorControllerWaypoint/SingleIntegratorControllerWaypoint.h>
-#include <scrimmage/math/State.h>
 
-REGISTER_PLUGIN(scrimmage::Controller, scrimmage::controller::SingleIntegratorControllerWaypoint, SingleIntegratorControllerWaypoint_plugin)
+REGISTER_PLUGIN(scrimmage::Controller, scrimmage::controller::SingleIntegratorControllerWaypoint,
+                SingleIntegratorControllerWaypoint_plugin)
 
 namespace scrimmage {
 namespace controller {
 
-void SingleIntegratorControllerWaypoint::init(std::map<std::string, std::string> &params) {
+void SingleIntegratorControllerWaypoint::init(std::map<std::string, std::string>& params) {
     input_pos_x_idx_ = vars_.declare(VariableIO::Type::position_x, VariableIO::Direction::In);
     input_pos_y_idx_ = vars_.declare(VariableIO::Type::position_y, VariableIO::Direction::In);
     input_pos_z_idx_ = vars_.declare(VariableIO::Type::position_z, VariableIO::Direction::In);
@@ -53,7 +54,7 @@ void SingleIntegratorControllerWaypoint::init(std::map<std::string, std::string>
 }
 
 bool SingleIntegratorControllerWaypoint::step(double t, double dt) {
-    const Eigen::Vector3d &p = state_->pos();
+    const Eigen::Vector3d& p = state_->pos();
 
     const double des_x = vars_.input(input_pos_x_idx_);
     const double des_y = vars_.input(input_pos_y_idx_);
@@ -66,5 +67,5 @@ bool SingleIntegratorControllerWaypoint::step(double t, double dt) {
     return true;
 }
 
-} // namespace controller
-} // namespace scrimmage
+}  // namespace controller
+}  // namespace scrimmage
diff --git a/src/plugins/controller/UUV6DOFLinearEnergy/UUV6DOFLinearEnergy.cpp b/src/plugins/controller/UUV6DOFLinearEnergy/UUV6DOFLinearEnergy.cpp
index 77fc09b9bf..17129d4728 100644
--- a/src/plugins/controller/UUV6DOFLinearEnergy/UUV6DOFLinearEnergy.cpp
+++ b/src/plugins/controller/UUV6DOFLinearEnergy/UUV6DOFLinearEnergy.cpp
@@ -30,13 +30,12 @@
  *
  */
 
-#include <scrimmage/plugins/controller/UUV6DOFLinearEnergy/UUV6DOFLinearEnergy.h>
-
-#include <scrimmage/plugin_manager/RegisterPlugin.h>
+#include <scrimmage/common/Utilities.h>
 #include <scrimmage/entity/Entity.h>
 #include <scrimmage/math/State.h>
-#include <scrimmage/common/Utilities.h>
 #include <scrimmage/parse/ParseUtils.h>
+#include <scrimmage/plugin_manager/RegisterPlugin.h>
+#include <scrimmage/plugins/controller/UUV6DOFLinearEnergy/UUV6DOFLinearEnergy.h>
 
 #include <iostream>
 #include <limits>
@@ -46,14 +45,13 @@ using std::endl;
 
 namespace sc = scrimmage;
 
-REGISTER_PLUGIN(scrimmage::Controller,
-                scrimmage::controller::UUV6DOFLinearEnergy,
+REGISTER_PLUGIN(scrimmage::Controller, scrimmage::controller::UUV6DOFLinearEnergy,
                 UUV6DOFLinearEnergy_plugin)
 
 namespace scrimmage {
 namespace controller {
 
-void UUV6DOFLinearEnergy::init(std::map<std::string, std::string> &params) {
+void UUV6DOFLinearEnergy::init(std::map<std::string, std::string>& params) {
     energy_ = sc::get<double>("energy_initial", params, energy_);
     energy_max_ = sc::get<double>("energy_max", params, energy_max_);
     energy_min_ = sc::get<double>("energy_min", params, energy_min_);
@@ -81,5 +79,5 @@ bool UUV6DOFLinearEnergy::step(double t, double dt) {
 
     return true;
 }
-} // namespace controller
-} // namespace scrimmage
+}  // namespace controller
+}  // namespace scrimmage
diff --git a/src/plugins/controller/UUV6DOFPIDController/UUV6DOFPIDController.cpp b/src/plugins/controller/UUV6DOFPIDController/UUV6DOFPIDController.cpp
index a87c8efb67..c3a21bf704 100644
--- a/src/plugins/controller/UUV6DOFPIDController/UUV6DOFPIDController.cpp
+++ b/src/plugins/controller/UUV6DOFPIDController/UUV6DOFPIDController.cpp
@@ -30,14 +30,13 @@
  *
  */
 
-#include <scrimmage/plugins/controller/UUV6DOFPIDController/UUV6DOFPIDController.h>
-
-#include <scrimmage/plugin_manager/RegisterPlugin.h>
+#include <scrimmage/common/Time.h>
+#include <scrimmage/common/Utilities.h>
 #include <scrimmage/entity/Entity.h>
 #include <scrimmage/math/State.h>
-#include <scrimmage/common/Utilities.h>
-#include <scrimmage/common/Time.h>
 #include <scrimmage/parse/ParseUtils.h>
+#include <scrimmage/plugin_manager/RegisterPlugin.h>
+#include <scrimmage/plugins/controller/UUV6DOFPIDController/UUV6DOFPIDController.h>
 
 #include <iostream>
 #include <limits>
@@ -45,8 +44,7 @@
 using std::cout;
 using std::endl;
 
-REGISTER_PLUGIN(scrimmage::Controller,
-                scrimmage::controller::UUV6DOFPIDController,
+REGISTER_PLUGIN(scrimmage::Controller, scrimmage::controller::UUV6DOFPIDController,
                 UUV6DOFPIDController_plugin)
 
 namespace scrimmage {
@@ -55,10 +53,12 @@ namespace controller {
 UUV6DOFPIDController::UUV6DOFPIDController() {
 }
 
-void UUV6DOFPIDController::init(std::map<std::string, std::string> &params) {
-    desired_altitude_idx_ = vars_.declare(VariableIO::Type::desired_altitude, VariableIO::Direction::In);
+void UUV6DOFPIDController::init(std::map<std::string, std::string>& params) {
+    desired_altitude_idx_ =
+        vars_.declare(VariableIO::Type::desired_altitude, VariableIO::Direction::In);
     desired_speed_idx_ = vars_.declare(VariableIO::Type::desired_speed, VariableIO::Direction::In);
-    desired_heading_idx_ = vars_.declare(VariableIO::Type::desired_heading, VariableIO::Direction::In);
+    desired_heading_idx_ =
+        vars_.declare(VariableIO::Type::desired_heading, VariableIO::Direction::In);
 
     throttle_idx_ = vars_.declare(VariableIO::Type::throttle, VariableIO::Direction::Out);
     elevator_idx_ = vars_.declare(VariableIO::Type::elevator, VariableIO::Direction::Out);
@@ -76,7 +76,6 @@ void UUV6DOFPIDController::init(std::map<std::string, std::string> &params) {
 }
 
 bool UUV6DOFPIDController::step(double t, double dt) {
-
     heading_pid_.set_setpoint(vars_.input(desired_heading_idx_));
     double u_rudder = heading_pid_.step(time_->dt(), state_->quat().yaw());
 
@@ -84,9 +83,10 @@ bool UUV6DOFPIDController::step(double t, double dt) {
     double u_throttle = speed_pid_.step(time_->dt(), state_->vel().norm());
 
     // Reconstruct original velocity vector (close, but not exact)
-    double x_vel = vars_.input(desired_speed_idx_) / sqrt(1 + pow(tan(vars_.input(desired_heading_idx_)), 2));
+    double x_vel =
+        vars_.input(desired_speed_idx_) / sqrt(1 + pow(tan(vars_.input(desired_heading_idx_)), 2));
     double y_vel = x_vel * tan(vars_.input(desired_heading_idx_));
-    Eigen::Vector3d vel(x_vel, y_vel, vars_.input(desired_altitude_idx_)-state_->pos()(2));
+    Eigen::Vector3d vel(x_vel, y_vel, vars_.input(desired_altitude_idx_) - state_->pos()(2));
     vel = vel.normalized() * vars_.input(desired_speed_idx_);
 
     // Track desired pitch
@@ -100,5 +100,5 @@ bool UUV6DOFPIDController::step(double t, double dt) {
 
     return true;
 }
-} // namespace controller
-} // namespace scrimmage
+}  // namespace controller
+}  // namespace scrimmage
diff --git a/src/plugins/controller/UnicycleControllerPoint/UnicycleControllerPoint.cpp b/src/plugins/controller/UnicycleControllerPoint/UnicycleControllerPoint.cpp
index 1ea0ef6c27..0daf2aa052 100644
--- a/src/plugins/controller/UnicycleControllerPoint/UnicycleControllerPoint.cpp
+++ b/src/plugins/controller/UnicycleControllerPoint/UnicycleControllerPoint.cpp
@@ -34,12 +34,13 @@
 #include <scrimmage/plugin_manager/RegisterPlugin.h>
 #include <scrimmage/plugins/controller/UnicycleControllerPoint/UnicycleControllerPoint.h>
 
-REGISTER_PLUGIN(scrimmage::Controller, scrimmage::controller::UnicycleControllerPoint, UnicycleControllerPoint_plugin)
+REGISTER_PLUGIN(scrimmage::Controller, scrimmage::controller::UnicycleControllerPoint,
+                UnicycleControllerPoint_plugin)
 
 namespace scrimmage {
 namespace controller {
 
-void UnicycleControllerPoint::init(std::map<std::string, std::string> &params) {
+void UnicycleControllerPoint::init(std::map<std::string, std::string>& params) {
     l_ = std::stod(params.at("l"));
     gain_ = std::stod(params.at("gain"));
 
@@ -76,5 +77,5 @@ bool UnicycleControllerPoint::step(double t, double dt) {
     vars_.output(velocity_z_idx_out_, gain_ * (des_z - z));
     return true;
 }
-} // namespace controller
-} // namespace scrimmage
+}  // namespace controller
+}  // namespace scrimmage
diff --git a/src/plugins/controller/UnicyclePID/UnicyclePID.cpp b/src/plugins/controller/UnicyclePID/UnicyclePID.cpp
index fb236f76cc..05d5c278ce 100644
--- a/src/plugins/controller/UnicyclePID/UnicyclePID.cpp
+++ b/src/plugins/controller/UnicyclePID/UnicyclePID.cpp
@@ -30,17 +30,15 @@
  *
  */
 
-#include <scrimmage/plugins/controller/UnicyclePID/UnicyclePID.h>
-
-#include <scrimmage/plugin_manager/RegisterPlugin.h>
+#include <scrimmage/common/Time.h>
+#include <scrimmage/common/Utilities.h>
 #include <scrimmage/entity/Entity.h>
 #include <scrimmage/math/State.h>
-#include <scrimmage/common/Utilities.h>
 #include <scrimmage/parse/ParseUtils.h>
-#include <scrimmage/common/Time.h>
-
-#include <scrimmage/proto/Shape.pb.h>
+#include <scrimmage/plugin_manager/RegisterPlugin.h>
+#include <scrimmage/plugins/controller/UnicyclePID/UnicyclePID.h>
 #include <scrimmage/proto/ProtoConversions.h>
+#include <scrimmage/proto/Shape.pb.h>
 
 #include <iostream>
 #include <limits>
@@ -50,9 +48,7 @@ using std::endl;
 
 namespace sc = scrimmage;
 
-REGISTER_PLUGIN(scrimmage::Controller,
-                scrimmage::controller::UnicyclePID,
-                UnicyclePID_plugin)
+REGISTER_PLUGIN(scrimmage::Controller, scrimmage::controller::UnicyclePID, UnicyclePID_plugin)
 
 namespace scrimmage {
 namespace controller {
@@ -60,12 +56,13 @@ namespace controller {
 UnicyclePID::UnicyclePID() {
 }
 
-void UnicyclePID::init(std::map<std::string, std::string> &params) {
+void UnicyclePID::init(std::map<std::string, std::string>& params) {
     show_shapes_ = sc::get<bool>("show_shapes", params, show_shapes_);
 
     desired_alt_idx_ = vars_.declare(VariableIO::Type::desired_altitude, VariableIO::Direction::In);
     desired_speed_idx_ = vars_.declare(VariableIO::Type::desired_speed, VariableIO::Direction::In);
-    desired_heading_idx_ = vars_.declare(VariableIO::Type::desired_heading, VariableIO::Direction::In);
+    desired_heading_idx_ =
+        vars_.declare(VariableIO::Type::desired_heading, VariableIO::Direction::In);
 
     // Is the motion model using speed or acceleration input control?
     std::string input_name = vars_.type_map().at(VariableIO::Type::speed);
@@ -100,7 +97,8 @@ bool UnicyclePID::step(double t, double dt) {
     vars_.output(turn_rate_idx_, heading_pid_.step(time_->dt(), state_->quat().yaw()));
 
     // Reconstruct original velocity vector (close, but not exact
-    double x_vel = vars_.input(desired_speed_idx_) / sqrt(1 + pow(tan(vars_.input(desired_heading_idx_)), 2));
+    double x_vel =
+        vars_.input(desired_speed_idx_) / sqrt(1 + pow(tan(vars_.input(desired_heading_idx_)), 2));
     double y_vel = x_vel * tan(vars_.input(desired_heading_idx_));
     Eigen::Vector3d vel(x_vel, y_vel, vars_.input(desired_alt_idx_) - state_->pos()(2));
     vel = vel.normalized() * vars_.input(desired_speed_idx_);
@@ -108,11 +106,11 @@ bool UnicyclePID::step(double t, double dt) {
     // Track desired pitch
     double desired_pitch = atan2(vel(2), vel.head<2>().norm());
     pitch_pid_.set_setpoint(desired_pitch);
-    vars_.output(pitch_rate_idx_, -pitch_pid_.step(time_->dt(), - state_->quat().pitch()));
+    vars_.output(pitch_rate_idx_, -pitch_pid_.step(time_->dt(), -state_->quat().pitch()));
 
     // Track zero roll
     roll_pid_.set_setpoint(0);
-    vars_.output(roll_rate_idx_, -roll_pid_.step(time_->dt(), - state_->quat().roll()));
+    vars_.output(roll_rate_idx_, -roll_pid_.step(time_->dt(), -state_->quat().roll()));
 
     if (show_shapes_) {
         line_shape_->set_persistent(true);
@@ -134,5 +132,5 @@ bool UnicyclePID::step(double t, double dt) {
 
     return true;
 }
-} // namespace controller
-} // namespace scrimmage
+}  // namespace controller
+}  // namespace scrimmage
diff --git a/src/plugins/interaction/Boundary/Boundary.cpp b/src/plugins/interaction/Boundary/Boundary.cpp
index 1d66003e3b..75120e63fd 100644
--- a/src/plugins/interaction/Boundary/Boundary.cpp
+++ b/src/plugins/interaction/Boundary/Boundary.cpp
@@ -30,44 +30,40 @@
  *
  */
 
-#include <scrimmage/plugin_manager/RegisterPlugin.h>
-#include <scrimmage/entity/Entity.h>
-#include <scrimmage/common/Utilities.h>
 #include <scrimmage/common/Random.h>
+#include <scrimmage/common/Utilities.h>
+#include <scrimmage/entity/Entity.h>
 #include <scrimmage/math/State.h>
 #include <scrimmage/parse/ParseUtils.h>
-#include <scrimmage/pubsub/Message.h>
-#include <scrimmage/pubsub/Publisher.h>
-#include <scrimmage/proto/ProtoConversions.h>
-
-#include <scrimmage/plugins/interaction/Boundary/BoundaryBase.h>
+#include <scrimmage/plugin_manager/RegisterPlugin.h>
 #include <scrimmage/plugins/interaction/Boundary/Boundary.h>
+#include <scrimmage/plugins/interaction/Boundary/BoundaryBase.h>
 #include <scrimmage/plugins/interaction/Boundary/Cuboid.h>
-#include <scrimmage/plugins/interaction/Boundary/Sphere.h>
 #include <scrimmage/plugins/interaction/Boundary/Plane.h>
+#include <scrimmage/plugins/interaction/Boundary/Sphere.h>
+#include <scrimmage/proto/ProtoConversions.h>
+#include <scrimmage/pubsub/Message.h>
+#include <scrimmage/pubsub/Publisher.h>
 
-#include <memory>
-#include <limits>
 #include <iostream>
+#include <limits>
+#include <memory>
 
 using std::cout;
 using std::endl;
 
 namespace sc = scrimmage;
 
-REGISTER_PLUGIN(scrimmage::EntityInteraction,
-                scrimmage::interaction::Boundary,
-                Boundary_plugin)
+REGISTER_PLUGIN(scrimmage::EntityInteraction, scrimmage::interaction::Boundary, Boundary_plugin)
 
 namespace scrimmage {
 namespace interaction {
 
-Boundary::Boundary() :
-    boundary_shape_(std::make_shared<scrimmage_proto::Shape>()) {
+Boundary::Boundary() : boundary_shape_(std::make_shared<scrimmage_proto::Shape>()) {
 }
 
-bool Boundary::init(std::map<std::string, std::string> &mission_params,
-                    std::map<std::string, std::string> &plugin_params) {
+bool Boundary::init(std::map<std::string, std::string>& mission_params,
+                    std::map<std::string, std::string>& plugin_params) {
     // Parse common boundary properties
 
     // Parse boundary name
@@ -81,9 +77,7 @@ bool Boundary::init(std::map<std::string, std::string> &mission_params,
     pub_boundary_ = advertise(network_name, "Boundary");
 
     // Parse boundary type
-    std::string type = sc::get<std::string>("type",
-                                            plugin_params,
-                                            "cuboid");
+    std::string type = sc::get<std::string>("type", plugin_params, "cuboid");
 
     if (type == "cuboid") {
         std::vector<double> center;
@@ -111,8 +105,8 @@ bool Boundary::init(std::map<std::string, std::string> &mission_params,
         sc::set(boundary_shape_->mutable_cuboid()->mutable_quat(), quat);
 
     } else if (type == "polyhedron") {
-        std::string polyhedron_points = sc::get<std::string>("polyhedron_points",
-                                                             plugin_params, "");
+        std::string polyhedron_points =
+            sc::get<std::string>("polyhedron_points", plugin_params, "");
 
         std::vector<std::vector<std::string>> vecs;
         if (!sc::get_vec_of_vecs(polyhedron_points, vecs)) {
@@ -126,7 +120,7 @@ bool Boundary::init(std::map<std::string, std::string> &mission_params,
                 cout << "Invalid vector size in: " << polyhedron_points << endl;
                 return false;
             }
-            sp::Vector3d *point = boundary_shape_->mutable_polyhedron()->add_point();
+            sp::Vector3d* point = boundary_shape_->mutable_polyhedron()->add_point();
             point->set_x(std::stod(vec[0]));
             point->set_y(std::stod(vec[1]));
             point->set_z(std::stod(vec[2]));
@@ -145,7 +139,7 @@ bool Boundary::init(std::map<std::string, std::string> &mission_params,
             std::cout << "Failed to parse 'center'" << endl;
             return false;
         }
-        sc::set(boundary_shape_->mutable_plane()->mutable_center(), sc::vec2eigen(center)); // TODO
+        sc::set(boundary_shape_->mutable_plane()->mutable_center(), sc::vec2eigen(center));  // TODO
 
         std::vector<double> rpy;
         if (!sc::get_vec<double>("rpy", plugin_params, " ,", rpy, 3)) {
@@ -177,8 +171,7 @@ bool Boundary::init(std::map<std::string, std::string> &mission_params,
     }
 
     if (sc::get<bool>("show_boundary", plugin_params, false)) {
-        double opacity = sc::get<double>("opacity", plugin_params,
-                                         1.0);
+        double opacity = sc::get<double>("opacity", plugin_params, 1.0);
         std::vector<int> color;
         if (!sc::get_vec("color", plugin_params, " ", color, 3)) {
             cout << "Failed to parse color" << endl;
@@ -192,12 +185,10 @@ bool Boundary::init(std::map<std::string, std::string> &mission_params,
         draw_shape(boundary_shape_);
     }
 
-
     return true;
 }
 
-bool Boundary::step_entity_interaction(std::list<sc::EntityPtr> &ents,
-                                                  double t, double dt) {
+bool Boundary::step_entity_interaction(std::list<sc::EntityPtr>& ents, double t, double dt) {
     if (!boundary_published_) {
         boundary_published_ = true;
         auto msg = std::make_shared<sc::Message<sp::Shape>>(*boundary_shape_);
@@ -207,38 +198,38 @@ bool Boundary::step_entity_interaction(std::list<sc::EntityPtr> &ents,
     return true;
 }
 
-std::shared_ptr<BoundaryBase> Boundary::make_boundary(const scrimmage_proto::Shape &shape) {
+std::shared_ptr<BoundaryBase> Boundary::make_boundary(const scrimmage_proto::Shape& shape) {
     std::shared_ptr<BoundaryBase> boundary = nullptr;
     switch (shape.oneof_type_case()) {
-    case sp::Shape::kTriangle:
-        break;
-    case sp::Shape::kPlane:
-        boundary = std::make_shared<Plane>(shape);
-        break;
-    case sp::Shape::kArrow:
-        break;
-    case sp::Shape::kCone:
-        break;
-    case sp::Shape::kLine:
-        break;
-    case sp::Shape::kPolygon:
-        break;
-    case sp::Shape::kPolydata:
-        break;
-    case sp::Shape::kCuboid:
-        boundary = std::make_shared<Cuboid>(shape);
-        break;
-    case sp::Shape::kPointcloud:
-        break;
-    case sp::Shape::kCircle:
-        break;
-    case sp::Shape::kSphere:
-        boundary = std::make_shared<Sphere>(shape);
-        break;
-    case sp::Shape::kText:
-        break;
-    default:
-        break;
+        case sp::Shape::kTriangle:
+            break;
+        case sp::Shape::kPlane:
+            boundary = std::make_shared<Plane>(shape);
+            break;
+        case sp::Shape::kArrow:
+            break;
+        case sp::Shape::kCone:
+            break;
+        case sp::Shape::kLine:
+            break;
+        case sp::Shape::kPolygon:
+            break;
+        case sp::Shape::kPolydata:
+            break;
+        case sp::Shape::kCuboid:
+            boundary = std::make_shared<Cuboid>(shape);
+            break;
+        case sp::Shape::kPointcloud:
+            break;
+        case sp::Shape::kCircle:
+            break;
+        case sp::Shape::kSphere:
+            boundary = std::make_shared<Sphere>(shape);
+            break;
+        case sp::Shape::kText:
+            break;
+        default:
+            break;
     }
 
     if (boundary != nullptr) {
@@ -249,6 +240,5 @@ std::shared_ptr<BoundaryBase> Boundary::make_boundary(const scrimmage_proto::Sha
     return boundary;
 }
 
-
-} // namespace interaction
-} // namespace scrimmage
+}  // namespace interaction
+}  // namespace scrimmage
diff --git a/src/plugins/interaction/BulletCollision/BulletCollision.cpp b/src/plugins/interaction/BulletCollision/BulletCollision.cpp
index 3997a39c3d..7976c2df0c 100644
--- a/src/plugins/interaction/BulletCollision/BulletCollision.cpp
+++ b/src/plugins/interaction/BulletCollision/BulletCollision.cpp
@@ -30,45 +30,43 @@
  *
  */
 
-#include <scrimmage/plugin_manager/RegisterPlugin.h>
-#include <scrimmage/entity/Entity.h>
-#include <scrimmage/common/Utilities.h>
-#include <scrimmage/common/Time.h>
+#include <scrimmage/autonomy/Autonomy.h>
+#include <scrimmage/common/RTree.h>
 #include <scrimmage/common/Shape.h>
-#include <scrimmage/parse/ParseUtils.h>
-#include <scrimmage/parse/MissionParse.h>
-#include <scrimmage/proto/Shape.pb.h>
-
+#include <scrimmage/common/Time.h>
+#include <scrimmage/common/Utilities.h>
+#include <scrimmage/entity/Entity.h>
 #include <scrimmage/math/State.h>
-
-#include <scrimmage/proto/ProtoConversions.h>
-#include <scrimmage/pubsub/Message.h>
+#include <scrimmage/motion/MotionModel.h>
 #include <scrimmage/msgs/Collision.pb.h>
 #include <scrimmage/msgs/Event.pb.h>
-#include <scrimmage/autonomy/Autonomy.h>
-#include <scrimmage/common/RTree.h>
-#include <scrimmage/sensor/Sensor.h>
-
-#include <scrimmage/motion/MotionModel.h>
 #include <scrimmage/network/Interface.h>
+#include <scrimmage/parse/MissionParse.h>
+#include <scrimmage/parse/ParseUtils.h>
+#include <scrimmage/plugin_manager/RegisterPlugin.h>
 #include <scrimmage/plugins/interaction/BulletCollision/BulletCollision.h>
 #include <scrimmage/plugins/sensor/RayTrace/RayTrace.h>
+#include <scrimmage/proto/ProtoConversions.h>
+#include <scrimmage/proto/Shape.pb.h>
+#include <scrimmage/pubsub/Message.h>
+#include <scrimmage/sensor/Sensor.h>
 
 #include <memory>
 
 #if ENABLE_VTK == 1
-#include <vtkSmartPointer.h>
 #include <vtkBYUReader.h>
 #include <vtkOBJReader.h>
 #include <vtkPLYReader.h>
-#include <vtkTriangle.h>
 #include <vtkPolyDataReader.h>
 #include <vtkSTLReader.h>
-#include <vtkXMLPolyDataReader.h>
+#include <vtkSmartPointer.h>
 #include <vtkSphereSource.h>
-#include <vtkTriangleFilter.h>
 #include <vtkTransform.h>
 #include <vtkTransformPolyDataFilter.h>
+#include <vtkTriangle.h>
+#include <vtkTriangleFilter.h>
+#include <vtkXMLPolyDataReader.h>
+
 #include <vtksys/SystemTools.hxx>
 #endif
 
@@ -78,7 +76,8 @@ namespace sp = scrimmage_proto;
 namespace pl = std::placeholders;
 using scrimmage::sensor::RayTrace;
 
-REGISTER_PLUGIN(scrimmage::EntityInteraction, scrimmage::interaction::BulletCollision, BulletCollision_plugin)
+REGISTER_PLUGIN(scrimmage::EntityInteraction, scrimmage::interaction::BulletCollision,
+                BulletCollision_plugin)
 
 namespace scrimmage {
 namespace interaction {
@@ -90,17 +89,16 @@ BulletCollision::BulletCollision() {
     bt_collision_configuration = new btDefaultCollisionConfiguration();
     bt_dispatcher = new btCollisionDispatcher(bt_collision_configuration);
 
-    btScalar sscene_size = (btScalar) scene_size_;
+    btScalar sscene_size = (btScalar)scene_size_;
     btVector3 worldAabbMin(-sscene_size, -sscene_size, -sscene_size);
     btVector3 worldAabbMax(sscene_size, sscene_size, sscene_size);
 
     // This is one type of broadphase, bullet has others that might be faster
     // depending on the application. true for disabling raycast accelerator.
-    bt_broadphase = new bt32BitAxisSweep3(worldAabbMin, worldAabbMax,
-                                          max_objects_, 0, true);
+    bt_broadphase = new bt32BitAxisSweep3(worldAabbMin, worldAabbMax, max_objects_, 0, true);
 
-    bt_collision_world = new btCollisionWorld(bt_dispatcher, bt_broadphase,
-            bt_collision_configuration);
+    bt_collision_world =
+        new btCollisionWorld(bt_dispatcher, bt_broadphase, bt_collision_configuration);
 }
 
 BulletCollision::~BulletCollision() {
@@ -110,33 +108,33 @@ BulletCollision::~BulletCollision() {
     delete bt_collision_configuration;
 }
 
-bool BulletCollision::init(std::map<std::string, std::string> &mission_params,
-                           std::map<std::string, std::string> &plugin_params) {
+bool BulletCollision::init(std::map<std::string, std::string>& mission_params,
+                           std::map<std::string, std::string>& plugin_params) {
     show_rays_ = sc::get<bool>("show_rays", plugin_params, false);
     enable_collision_detection_ = sc::get<bool>("enable_collision_detection", plugin_params, true);
     enable_ray_tracing_ = sc::get<bool>("enable_ray_tracing", plugin_params, true);
 
-    remove_on_collision_ = sc::get<bool>("remove_on_collision", plugin_params,
-                                         remove_on_collision_);
-    show_collision_shapes_ = sc::get<bool>("show_collision_shapes", plugin_params,
-                                           show_collision_shapes_);
-    enable_ground_plane_ = sc::get<bool>("enable_ground_plane", plugin_params,
-                                         enable_ground_plane_);
-    ground_plane_height_ = sc::get<double>("ground_plane_height", plugin_params,
-                                           ground_plane_height_);
-    enable_terrain_ = sc::get<bool>("enable_terrain", plugin_params,
-                                    enable_terrain_);
-
-    enable_team_collisions_ = get<bool>("enable_team_collisions",
-                                        plugin_params, enable_team_collisions_);
-    enable_non_team_collisions_ = get<bool>("enable_non_team_collisions",
-                                            plugin_params, enable_non_team_collisions_);
+    remove_on_collision_ =
+        sc::get<bool>("remove_on_collision", plugin_params, remove_on_collision_);
+    show_collision_shapes_ =
+        sc::get<bool>("show_collision_shapes", plugin_params, show_collision_shapes_);
+    enable_ground_plane_ =
+        sc::get<bool>("enable_ground_plane", plugin_params, enable_ground_plane_);
+    ground_plane_height_ =
+        sc::get<double>("ground_plane_height", plugin_params, ground_plane_height_);
+    enable_terrain_ = sc::get<bool>("enable_terrain", plugin_params, enable_terrain_);
+
+    enable_team_collisions_ =
+        get<bool>("enable_team_collisions", plugin_params, enable_team_collisions_);
+    enable_non_team_collisions_ =
+        get<bool>("enable_non_team_collisions", plugin_params, enable_non_team_collisions_);
 
     collision_pub_ = advertise("GlobalNetwork", "GroundCollision");
     team_collision_pub_ = advertise("GlobalNetwork", "TeamCollision");
     non_team_collision_pub_ = advertise("GlobalNetwork", "NonTeamCollision");
 
-    publish_on_local_networks_ = get("publish_on_local_networks", plugin_params, publish_on_local_networks_);
+    publish_on_local_networks_ =
+        get("publish_on_local_networks", plugin_params, publish_on_local_networks_);
     pcl_network_name_ = get("pcl_network_name", plugin_params, pcl_network_name_);
     pcl_topic_name_ = get("pcl_topic_name", plugin_params, pcl_topic_name_);
     prepend_pcl_topic_with_id_ = get("prepend_pcl_topic_with_id", plugin_params, false);
@@ -147,39 +145,36 @@ bool BulletCollision::init(std::map<std::string, std::string> &mission_params,
 
     // Enable the service interface for ray tracing
 
-    auto ent_gen_cb = [&] (scrimmage::MessagePtr<sm::EntityGenerated> msg) {
+    auto ent_gen_cb = [&](scrimmage::MessagePtr<sm::EntityGenerated> msg) {
         auto pc_desc = std::make_unique<PointCloudDescription>();
 
         int id = msg->data.entity_id();
 
-        sc::EntityPtr &ent = (*id_to_ent_map_)[id];
+        sc::EntityPtr& ent = (*id_to_ent_map_)[id];
 
         btCollisionObject* coll_object = new btCollisionObject();
         coll_object->setUserIndex(id);
         coll_object->getWorldTransform().setOrigin(btVector3(
-            (btScalar) ent->state_truth()->pos()(0),
-            (btScalar) ent->state_truth()->pos()(1),
-            (btScalar) ent->state_truth()->pos()(2)));
+            (btScalar)ent->state_truth()->pos()(0), (btScalar)ent->state_truth()->pos()(1),
+            (btScalar)ent->state_truth()->pos()(2)));
 
-        btSphereShape * sphere_shape = new btSphereShape(ent->radius());  // TODO: memory management
+        btSphereShape* sphere_shape = new btSphereShape(ent->radius());  // TODO: memory management
         coll_object->setCollisionShape(sphere_shape);
         bt_collision_world->addCollisionObject(coll_object);
 
         objects_[id].object = coll_object;
 
         if (show_collision_shapes_) {
-            objects_[id].shape = sc::shape::make_sphere(
-                ent->state_truth()->pos(), ent->radius(),
-                Eigen::Vector3d(0, 0, 255), 0.30);
+            objects_[id].shape = sc::shape::make_sphere(ent->state_truth()->pos(), ent->radius(),
+                                                        Eigen::Vector3d(0, 0, 255), 0.30);
             draw_shape(objects_[id].shape);
         }
 
         if (enable_ray_tracing_) {
             // What types of sensors need to be attached to this entity?
-            for (auto &kv : ent->sensors()) {
+            for (auto& kv : ent->sensors()) {
                 if (kv.second->type() == "Ray") {
-                    std::shared_ptr<RayTrace> rs =
-                        std::dynamic_pointer_cast<RayTrace>(kv.second);
+                    std::shared_ptr<RayTrace> rs = std::dynamic_pointer_cast<RayTrace>(kv.second);
                     // Check whether automatic ray tracing should be done
                     if (rs->automatic_ray_tracing() == true) {
                         std::string topic = kv.second->name() + "/" + pcl_topic_name_;
@@ -208,12 +203,14 @@ bool BulletCollision::init(std::map<std::string, std::string> &mission_params,
                             pcl.max_range = rs->max_range();
                             pcl.min_range = rs->min_range();
                             pcl.max_sample_rate = rs->max_sample_rate();
-                            // Get the rays (copied internally, so can just assign here), using the model name
+                            // Get the rays (copied internally, so can just assign here), using the
+                            // model name
                             //  so the dirty flag can be checked later.
                             pcl.rays = rs->rays();
-                            for (auto &ray : pcl.rays) {
+                            for (auto& ray : pcl.rays) {
                                 Eigen::Vector3d r(rs->max_range(), 0, 0);
-                                sc::Quaternion rot_vert(Eigen::Vector3d(0, 1, 0), ray.elevation_rad);
+                                sc::Quaternion rot_vert(Eigen::Vector3d(0, 1, 0),
+                                                        ray.elevation_rad);
                                 r = rot_vert.rotate(r);
 
                                 sc::Quaternion rot_horiz(Eigen::Vector3d(0, 0, 1), ray.azimuth_rad);
@@ -227,13 +224,15 @@ bool BulletCollision::init(std::map<std::string, std::string> &mission_params,
                             if (show_rays_) {
                                 // Construct the ray shapes, but don't draw them
                                 // yet.
-                                for (unsigned int i = 0;
-                                     i < pc_desc->point_cloud.points.size(); i++) {
+                                for (unsigned int i = 0; i < pc_desc->point_cloud.points.size();
+                                     i++) {
                                     std::shared_ptr<sp::Shape> line(new sp::Shape);
                                     sc::set(line->mutable_color(), 255, 0, 0);
                                     line->set_opacity(1.0);
-                                    sc::set(line->mutable_line()->mutable_start(), Eigen::Vector3d(0, 0, 0));
-                                    sc::set(line->mutable_line()->mutable_end(), Eigen::Vector3d(0, 0, 0));
+                                    sc::set(line->mutable_line()->mutable_start(),
+                                            Eigen::Vector3d(0, 0, 0));
+                                    sc::set(line->mutable_line()->mutable_end(),
+                                            Eigen::Vector3d(0, 0, 0));
                                     pc_desc->shapes.push_back(line);
                                 }
                             }
@@ -248,20 +247,21 @@ bool BulletCollision::init(std::map<std::string, std::string> &mission_params,
     };
     subscribe<sm::EntityGenerated>("GlobalNetwork", "EntityGenerated", ent_gen_cb);
 
-    auto shape_gen_cb = [&] (scrimmage::MessagePtr<sp::Shapes> msg) {
+    auto shape_gen_cb = [&](scrimmage::MessagePtr<sp::Shapes> msg) {
         for (int i = 0; i < msg->data.shape_size(); i++) {
             if (msg->data.shape(i).oneof_type_case() == sp::Shape::kCuboid) {
                 btVector3 xyz(btScalar(msg->data.shape(i).cuboid().x_length() / 2.0),
                               btScalar(msg->data.shape(i).cuboid().y_length() / 2.0),
                               btScalar(msg->data.shape(i).cuboid().z_length() / 2.0));
 
-                btBoxShape *wall = new btBoxShape(xyz);
+                btBoxShape* wall = new btBoxShape(xyz);
 
                 btCollisionObject* coll_object = new btCollisionObject();
                 coll_object->setCollisionShape(wall);
-                coll_object->getWorldTransform().setOrigin(btVector3((btScalar) msg->data.shape(i).cuboid().center().x(),
-                        (btScalar) msg->data.shape(i).cuboid().center().y(),
-                        (btScalar) msg->data.shape(i).cuboid().center().z()));
+                coll_object->getWorldTransform().setOrigin(
+                    btVector3((btScalar)msg->data.shape(i).cuboid().center().x(),
+                              (btScalar)msg->data.shape(i).cuboid().center().y(),
+                              (btScalar)msg->data.shape(i).cuboid().center().z()));
                 bt_collision_world->addCollisionObject(coll_object);
             }
         }
@@ -275,8 +275,7 @@ bool BulletCollision::init(std::map<std::string, std::string> &mission_params,
         coll_object->setUserIndex(0);
         coll_object->setCollisionShape(ground_shape);
         coll_object->getWorldTransform().setOrigin(
-            btVector3((btScalar) 0, (btScalar) 0,
-                      (btScalar) ground_plane_height_));
+            btVector3((btScalar)0, (btScalar)0, (btScalar)ground_plane_height_));
         bt_collision_world->addCollisionObject(coll_object);
     }
 
@@ -286,65 +285,60 @@ bool BulletCollision::init(std::map<std::string, std::string> &mission_params,
         if (utm_terrain->enable_terrain() == true) {
             // Read the terrain mesh file
             vtkSmartPointer<vtkPolyData> polyData;
-            std::string extension = vtksys::SystemTools::GetFilenameLastExtension(std::string(utm_terrain->poly_data_file().c_str()));
+            std::string extension = vtksys::SystemTools::GetFilenameLastExtension(
+                std::string(utm_terrain->poly_data_file().c_str()));
             if (extension == ".ply") {
-                vtkSmartPointer<vtkPLYReader> reader =
-                        vtkSmartPointer<vtkPLYReader>::New();
+                vtkSmartPointer<vtkPLYReader> reader = vtkSmartPointer<vtkPLYReader>::New();
                 reader->SetFileName(utm_terrain->poly_data_file().c_str());
                 reader->Update();
                 polyData = reader->GetOutput();
             } else if (extension == ".vtp") {
                 vtkSmartPointer<vtkXMLPolyDataReader> reader =
-                        vtkSmartPointer<vtkXMLPolyDataReader>::New();
+                    vtkSmartPointer<vtkXMLPolyDataReader>::New();
                 reader->SetFileName(utm_terrain->poly_data_file().c_str());
                 reader->Update();
                 polyData = reader->GetOutput();
             } else if (extension == ".obj") {
-                vtkSmartPointer<vtkOBJReader> reader =
-                        vtkSmartPointer<vtkOBJReader>::New();
+                vtkSmartPointer<vtkOBJReader> reader = vtkSmartPointer<vtkOBJReader>::New();
                 reader->SetFileName(utm_terrain->poly_data_file().c_str());
                 reader->Update();
                 polyData = reader->GetOutput();
             } else if (extension == ".stl") {
-                vtkSmartPointer<vtkSTLReader> reader =
-                        vtkSmartPointer<vtkSTLReader>::New();
+                vtkSmartPointer<vtkSTLReader> reader = vtkSmartPointer<vtkSTLReader>::New();
                 reader->SetFileName(utm_terrain->poly_data_file().c_str());
                 reader->Update();
                 polyData = reader->GetOutput();
             } else if (extension == ".vtk") {
                 vtkSmartPointer<vtkPolyDataReader> reader =
-                        vtkSmartPointer<vtkPolyDataReader>::New();
+                    vtkSmartPointer<vtkPolyDataReader>::New();
                 reader->SetFileName(utm_terrain->poly_data_file().c_str());
                 reader->Update();
                 polyData = reader->GetOutput();
             } else if (extension == ".g") {
-                vtkSmartPointer<vtkBYUReader> reader =
-                        vtkSmartPointer<vtkBYUReader>::New();
+                vtkSmartPointer<vtkBYUReader> reader = vtkSmartPointer<vtkBYUReader>::New();
                 reader->SetGeometryFileName(utm_terrain->poly_data_file().c_str());
                 reader->Update();
                 polyData = reader->GetOutput();
             } else {
-                vtkSmartPointer<vtkSphereSource> source =
-                        vtkSmartPointer<vtkSphereSource>::New();
+                vtkSmartPointer<vtkSphereSource> source = vtkSmartPointer<vtkSphereSource>::New();
                 source->Update();
                 polyData = source->GetOutput();
             }
             // Update the location
-            vtkSmartPointer<vtkTransform> translation =
-                    vtkSmartPointer<vtkTransform>::New();
-            translation->Translate(-utm_terrain->x_translate(),
-                                   -utm_terrain->y_translate(),
+            vtkSmartPointer<vtkTransform> translation = vtkSmartPointer<vtkTransform>::New();
+            translation->Translate(-utm_terrain->x_translate(), -utm_terrain->y_translate(),
                                    -utm_terrain->z_translate());
 
             vtkSmartPointer<vtkTransformPolyDataFilter> transformFilter =
-                    vtkSmartPointer<vtkTransformPolyDataFilter>::New();
+                vtkSmartPointer<vtkTransformPolyDataFilter>::New();
             transformFilter->SetInputData(polyData);
             transformFilter->SetTransform(translation);
             transformFilter->Update();
             vtkSmartPointer<vtkPolyData> transformedData = transformFilter->GetOutput();
 
             // Next, convert it to a triangular mesh
-            vtkSmartPointer<vtkTriangleFilter> triFilter = vtkSmartPointer<vtkTriangleFilter>::New();
+            vtkSmartPointer<vtkTriangleFilter> triFilter =
+                vtkSmartPointer<vtkTriangleFilter>::New();
             triFilter->SetInputData(transformedData);
             triFilter->Update();
             vtkSmartPointer<vtkPolyData> triangularData = triFilter->GetOutput();
@@ -362,11 +356,15 @@ bool BulletCollision::init(std::map<std::string, std::string> &mission_params,
                 triangle->GetPoints()->GetPoint(1, p1);
                 triangle->GetPoints()->GetPoint(2, p2);
                 // Create the new triangle
-                triangles->addTriangle(btVector3(p0[0], p0[1], p0[2]), btVector3(p1[0], p1[1], p1[2]), btVector3(p2[0], p2[1], p2[2]));
+                triangles->addTriangle(btVector3(p0[0], p0[1], p0[2]),
+                                       btVector3(p1[0], p1[1], p1[2]),
+                                       btVector3(p2[0], p2[1], p2[2]));
             }
             btCollisionShape* mesh = new btBvhTriangleMeshShape(triangles, true, true);
-            btDefaultMotionState* motionState = new btDefaultMotionState(btTransform(btQuaternion(0, 0, 0, 1), btVector3(0, 0, 0)));
-            btRigidBody::btRigidBodyConstructionInfo rigidBodyConstructionInfo(0.0f, motionState, mesh, btVector3(0, 0, 0));
+            btDefaultMotionState* motionState =
+                new btDefaultMotionState(btTransform(btQuaternion(0, 0, 0, 1), btVector3(0, 0, 0)));
+            btRigidBody::btRigidBodyConstructionInfo rigidBodyConstructionInfo(
+                0.0f, motionState, mesh, btVector3(0, 0, 0));
             btRigidBody* rigidBodyTerrain = new btRigidBody(rigidBodyConstructionInfo);
             rigidBodyTerrain->setFriction(btScalar(0.9));
             bt_collision_world->addCollisionObject(rigidBodyTerrain);
@@ -376,17 +374,15 @@ bool BulletCollision::init(std::map<std::string, std::string> &mission_params,
     return true;
 }
 
-bool BulletCollision::step_entity_interaction(std::list<sc::EntityPtr> &ents,
-                                              double t, double dt) {
+bool BulletCollision::step_entity_interaction(std::list<sc::EntityPtr>& ents, double t, double dt) {
     // Update positions of all objects
-    for (auto &kv : *id_to_ent_map_) {
+    for (auto& kv : *id_to_ent_map_) {
         auto it_object = objects_.find(kv.first);
         if (it_object != objects_.end()) {
-            sc::EntityPtr &ent = kv.second;
+            sc::EntityPtr& ent = kv.second;
             it_object->second.object->getWorldTransform().setOrigin(btVector3(
-                (btScalar) ent->state_truth()->pos()(0),
-                (btScalar) ent->state_truth()->pos()(1),
-                (btScalar) ent->state_truth()->pos()(2)));
+                (btScalar)ent->state_truth()->pos()(0), (btScalar)ent->state_truth()->pos()(1),
+                (btScalar)ent->state_truth()->pos()(2)));
 
             if (show_collision_shapes_) {
                 sc::set(it_object->second.shape->mutable_sphere()->mutable_center(),
@@ -398,7 +394,7 @@ bool BulletCollision::step_entity_interaction(std::list<sc::EntityPtr> &ents,
 
     // If an entity no longer exists in the ent map, remove it's shape from the
     // bullet environment
-    for (auto &kv : objects_) {
+    for (auto& kv : objects_) {
         auto it_ent = id_to_ent_map_->find(kv.first);
         if (it_ent == id_to_ent_map_->end()) {
             remove_object(kv.first);
@@ -406,13 +402,13 @@ bool BulletCollision::step_entity_interaction(std::list<sc::EntityPtr> &ents,
     }
 
     // For each entity's ray-based sensors, compute point clouds
-    for (auto &kv : pc_descs_) {
-        sc::EntityPtr &own_ent = (*id_to_ent_map_)[kv.first];
+    for (auto& kv : pc_descs_) {
+        sc::EntityPtr& own_ent = (*id_to_ent_map_)[kv.first];
         Eigen::Vector3d own_pos = own_ent->state_truth()->pos();
 
         // For each ray sensor on a single entity
-        for (auto &kv2 : kv.second) {
-            sensor::RayTrace::PointCloud &pc = kv2.second->point_cloud;
+        for (auto& kv2 : kv.second) {
+            sensor::RayTrace::PointCloud& pc = kv2.second->point_cloud;
 
             if (t > (pc.max_sample_rate + kv2.second->last_update_time)) {
                 kv2.second->last_update_time = time_->t();
@@ -424,23 +420,22 @@ bool BulletCollision::step_entity_interaction(std::list<sc::EntityPtr> &ents,
 
                 // Compute transformation matrix from entity's frame to sensor's
                 // frame.
-                sc::SensorPtr &sensor = own_ent->sensors()[kv2.first];
-                Eigen::Matrix4d tf_m = own_ent->state_truth()->tf_matrix(false) *
-                                       sensor->transform()->tf_matrix();
+                sc::SensorPtr& sensor = own_ent->sensors()[kv2.first];
+                Eigen::Matrix4d tf_m =
+                    own_ent->state_truth()->tf_matrix(false) * sensor->transform()->tf_matrix();
 
                 // For each ray in the sensor
                 unsigned int i = 0;
-                for (RayTrace::PCPoint &pcpoint : pc.points) {
+                for (RayTrace::PCPoint& pcpoint : pc.points) {
                     Eigen::Vector3d original_ray = pcpoint.point;
                     // Transform sensor's origin to world coordinates
                     Eigen::Vector4d sensor_pos = tf_m * Eigen::Vector4d(0, 0, 0, 1);
                     Eigen::Vector3d sensor_pos_w = sensor_pos.head<3>() + own_pos;
 
                     // Transform ray's end point to world coordinates
-                    Eigen::Vector4d ray = tf_m * Eigen::Vector4d(original_ray(0),
-                                          original_ray(1),
-                                          original_ray(2),
-                                          1);
+                    Eigen::Vector4d ray =
+                        tf_m
+                        * Eigen::Vector4d(original_ray(0), original_ray(1), original_ray(2), 1);
                     Eigen::Vector3d ray_w = ray.head<3>() + own_pos;
 
                     // Create bullet vectors
@@ -457,15 +452,14 @@ bool BulletCollision::step_entity_interaction(std::list<sc::EntityPtr> &ents,
                     // collision occurred.
                     Eigen::Vector3d ray_end;
                     if (res.hasHit()) {
-                        Eigen::Vector3d hit_point(res.m_hitPointWorld.x(),
-                                                  res.m_hitPointWorld.y(),
+                        Eigen::Vector3d hit_point(res.m_hitPointWorld.x(), res.m_hitPointWorld.y(),
                                                   res.m_hitPointWorld.z());
-                        Eigen::Vector3d return_vec = original_ray.normalized() *
-                                                     (hit_point - sensor_pos_w).norm();
+                        Eigen::Vector3d return_vec =
+                            original_ray.normalized() * (hit_point - sensor_pos_w).norm();
                         msg->data.points.push_back(
                             RayTrace::PCPoint(return_vec, 255,
-                                              ((return_vec.norm() > msg->data.max_range) ||
-                                               (return_vec.norm() < msg->data.min_range))));
+                                              ((return_vec.norm() > msg->data.max_range)
+                                               || (return_vec.norm() < msg->data.min_range))));
                         ray_end = hit_point;
                     } else {
                         msg->data.points.push_back(RayTrace::PCPoint(original_ray, 255, true));
@@ -473,7 +467,7 @@ bool BulletCollision::step_entity_interaction(std::list<sc::EntityPtr> &ents,
                     }
 
                     if (show_rays_) {
-                        std::unique_ptr<PointCloudDescription> &pc_desc = kv2.second;
+                        std::unique_ptr<PointCloudDescription>& pc_desc = kv2.second;
                         if (res.hasHit()) {
                             sc::set(pc_desc->shapes[i]->mutable_color(), 255, 0, 0);
                             pc_desc->shapes[i]->set_opacity(1.0);
@@ -499,10 +493,14 @@ bool BulletCollision::step_entity_interaction(std::list<sc::EntityPtr> &ents,
 
         // For each contact manifold
         for (int i = 0; i < numManifolds; i++) {
-            btPersistentManifold* contactManifold = bt_collision_world->getDispatcher()->getManifoldByIndexInternal(i);
-            const btCollisionObject* obA = static_cast<const btCollisionObject*>(contactManifold->getBody0());
-            const btCollisionObject* obB = static_cast<const btCollisionObject*>(contactManifold->getBody1());
-            contactManifold->refreshContactPoints(obA->getWorldTransform(), obB->getWorldTransform());
+            btPersistentManifold* contactManifold =
+                bt_collision_world->getDispatcher()->getManifoldByIndexInternal(i);
+            const btCollisionObject* obA =
+                static_cast<const btCollisionObject*>(contactManifold->getBody0());
+            const btCollisionObject* obB =
+                static_cast<const btCollisionObject*>(contactManifold->getBody1());
+            contactManifold->refreshContactPoints(obA->getWorldTransform(),
+                                                  obB->getWorldTransform());
             int numContacts = contactManifold->getNumContacts();
 
             // For each contact point in that manifold
@@ -510,8 +508,7 @@ bool BulletCollision::step_entity_interaction(std::list<sc::EntityPtr> &ents,
                 // Get the contact information
                 btManifoldPoint& pt = contactManifold->getContactPoint(j);
 
-                Eigen::Vector3d normal_B(pt.m_normalWorldOnB.x(),
-                                         pt.m_normalWorldOnB.y(),
+                Eigen::Vector3d normal_B(pt.m_normalWorldOnB.x(), pt.m_normalWorldOnB.y(),
                                          pt.m_normalWorldOnB.z());
 
                 if (id_to_ent_map_->count(obB->getUserIndex()) > 0) {
@@ -585,7 +582,7 @@ bool BulletCollision::step_entity_interaction(std::list<sc::EntityPtr> &ents,
 }
 
 bool BulletCollision::get_ray_tracing(scrimmage::MessageBasePtr request,
-                                      scrimmage::MessageBasePtr &response) {
+                                      scrimmage::MessageBasePtr& response) {
     auto request_cast = std::dynamic_pointer_cast<sc::Message<RayTrace::PointCloudWithId>>(request);
 
     if (request_cast == nullptr) {
@@ -595,20 +592,20 @@ bool BulletCollision::get_ray_tracing(scrimmage::MessageBasePtr request,
 
     int entity_id = request_cast->data.entity_id;
     std::string sensor_name = request_cast->data.sensor_name;
-    sc::EntityPtr &own_ent = (*id_to_ent_map_)[entity_id];
+    sc::EntityPtr& own_ent = (*id_to_ent_map_)[entity_id];
     Eigen::Vector3d own_pos = own_ent->state_truth()->pos();
     // Compute transformation matrix from entity's frame to sensor's
     // frame.
-    sc::SensorPtr &sensor = own_ent->sensors()[sensor_name];
-    Eigen::Matrix4d tf_m = own_ent->state_truth()->tf_matrix(false) *
-                           sensor->transform()->tf_matrix();
+    sc::SensorPtr& sensor = own_ent->sensors()[sensor_name];
+    Eigen::Matrix4d tf_m =
+        own_ent->state_truth()->tf_matrix(false) * sensor->transform()->tf_matrix();
     auto response_cast = std::make_shared<sc::Message<RayTrace::PointCloud>>();
     response_cast->data.max_range = request_cast->data.max_range;
     response_cast->data.min_range = request_cast->data.min_range;
     response_cast->data.rays = request_cast->data.get_rays();
 
     // Create the points from the rays
-    for (auto &ray : response_cast->data.rays) {
+    for (auto& ray : response_cast->data.rays) {
         Eigen::Vector3d r(response_cast->data.max_range, 0, 0);
         sc::Quaternion rot_vert(Eigen::Vector3d(0, 1, 0), ray.elevation_rad);
         r = rot_vert.rotate(r);
@@ -620,7 +617,7 @@ bool BulletCollision::get_ray_tracing(scrimmage::MessageBasePtr request,
     }
 
     // For each ray in the sensor
-    for (RayTrace::PCPoint &pcpoint : request_cast->data.points) {
+    for (RayTrace::PCPoint& pcpoint : request_cast->data.points) {
         Eigen::Vector3d original_ray = pcpoint.point;
         // Transform sensor's origin to world coordinates
         Eigen::Vector4d sensor_pos = tf_m * Eigen::Vector4d(0, 0, 0, 1);
@@ -629,10 +626,8 @@ bool BulletCollision::get_ray_tracing(scrimmage::MessageBasePtr request,
         Eigen::Vector3d ray_w;
         if (request_cast->data.world_frame == false) {
             // Transform ray's end point to world coordinates
-            Eigen::Vector4d ray = tf_m * Eigen::Vector4d(original_ray(0),
-                                  original_ray(1),
-                                  original_ray(2),
-                                  1);
+            Eigen::Vector4d ray =
+                tf_m * Eigen::Vector4d(original_ray(0), original_ray(1), original_ray(2), 1);
             ray_w = ray.head<3>() + own_pos;
         } else {
             // Already in world frame
@@ -653,15 +648,14 @@ bool BulletCollision::get_ray_tracing(scrimmage::MessageBasePtr request,
         // collision occurred.
         Eigen::Vector3d ray_end;
         if (res.hasHit()) {
-            Eigen::Vector3d hit_point(res.m_hitPointWorld.x(),
-                                      res.m_hitPointWorld.y(),
+            Eigen::Vector3d hit_point(res.m_hitPointWorld.x(), res.m_hitPointWorld.y(),
                                       res.m_hitPointWorld.z());
-            Eigen::Vector3d return_vec = original_ray.normalized() *
-                                         (hit_point - sensor_pos_w).norm();
+            Eigen::Vector3d return_vec =
+                original_ray.normalized() * (hit_point - sensor_pos_w).norm();
             response_cast->data.points.push_back(
                 RayTrace::PCPoint(return_vec, 255,
-                                  ((return_vec.norm() > response_cast->data.max_range) ||
-                                   (return_vec.norm() < response_cast->data.min_range))));
+                                  ((return_vec.norm() > response_cast->data.max_range)
+                                   || (return_vec.norm() < response_cast->data.min_range))));
             ray_end = hit_point;
         } else {
             response_cast->data.points.push_back(RayTrace::PCPoint(original_ray, 255, true));
@@ -675,7 +669,7 @@ bool BulletCollision::get_ray_tracing(scrimmage::MessageBasePtr request,
     return true;
 }
 
-std::pair<bool, scrimmage::EntityPtr> BulletCollision::get_entity(const int &id) {
+std::pair<bool, scrimmage::EntityPtr> BulletCollision::get_entity(const int& id) {
     auto it_ent = id_to_ent_map_->find(id);
     if (it_ent != id_to_ent_map_->end()) {
         return std::make_pair(true, it_ent->second);
@@ -684,12 +678,12 @@ std::pair<bool, scrimmage::EntityPtr> BulletCollision::get_entity(const int &id)
     }
 }
 
-void BulletCollision::remove_entity_object(const int &id) {
+void BulletCollision::remove_entity_object(const int& id) {
     entity_collision(id);
     remove_object(id);
 }
 
-void BulletCollision::entity_collision(const int &id) {
+void BulletCollision::entity_collision(const int& id) {
     // Call the collision() function to remove entity
     auto it_ent = id_to_ent_map_->find(id);
     if (it_ent != id_to_ent_map_->end()) {
@@ -697,7 +691,7 @@ void BulletCollision::entity_collision(const int &id) {
     }
 }
 
-void BulletCollision::remove_object(const int &id) {
+void BulletCollision::remove_object(const int& id) {
     auto it_object = objects_.find(id);
     if (it_object != objects_.end()) {
         // Remove the object shape if it exists
@@ -714,8 +708,7 @@ void BulletCollision::remove_object(const int &id) {
     }
 }
 
-bool BulletCollision::collision_exists(std::list<sc::EntityPtr> &ents,
-                                       Eigen::Vector3d &p) {
+bool BulletCollision::collision_exists(std::list<sc::EntityPtr>& ents, Eigen::Vector3d& p) {
     return false;
 }
 }  // namespace interaction
diff --git a/src/plugins/interaction/CaptureInBoundaryInteraction/CaptureInBoundaryInteraction.cpp b/src/plugins/interaction/CaptureInBoundaryInteraction/CaptureInBoundaryInteraction.cpp
index b603b6eae7..e0029feb06 100644
--- a/src/plugins/interaction/CaptureInBoundaryInteraction/CaptureInBoundaryInteraction.cpp
+++ b/src/plugins/interaction/CaptureInBoundaryInteraction/CaptureInBoundaryInteraction.cpp
@@ -30,25 +30,23 @@
  *
  */
 
-#include <scrimmage/plugins/interaction/CaptureInBoundaryInteraction/CaptureInBoundaryInteraction.h>
-
-#include <scrimmage/plugin_manager/RegisterPlugin.h>
-#include <scrimmage/entity/Entity.h>
-#include <scrimmage/common/Utilities.h>
 #include <scrimmage/common/RTree.h>
 #include <scrimmage/common/Time.h>
+#include <scrimmage/common/Utilities.h>
+#include <scrimmage/entity/Entity.h>
 #include <scrimmage/math/State.h>
+#include <scrimmage/msgs/Capture.pb.h>
 #include <scrimmage/parse/ParseUtils.h>
+#include <scrimmage/plugin_manager/RegisterPlugin.h>
+#include <scrimmage/plugins/interaction/Boundary/Boundary.h>
+#include <scrimmage/plugins/interaction/CaptureInBoundaryInteraction/CaptureInBoundaryInteraction.h>
 #include <scrimmage/pubsub/Message.h>
 #include <scrimmage/pubsub/Publisher.h>
 #include <scrimmage/pubsub/Subscriber.h>
-#include <scrimmage/msgs/Capture.pb.h>
-
-#include <scrimmage/plugins/interaction/Boundary/Boundary.h>
 
-#include <memory>
-#include <limits>
 #include <iostream>
+#include <limits>
+#include <memory>
 
 using std::cout;
 using std::endl;
@@ -58,25 +56,23 @@ namespace sci = scrimmage::interaction;
 namespace sm = scrimmage_msgs;
 namespace sp = scrimmage_proto;
 
-REGISTER_PLUGIN(scrimmage::EntityInteraction,
-                scrimmage::interaction::CaptureInBoundaryInteraction,
+REGISTER_PLUGIN(scrimmage::EntityInteraction, scrimmage::interaction::CaptureInBoundaryInteraction,
                 CaptureInBoundaryInteraction_plugin)
 
 namespace scrimmage {
 namespace interaction {
 
-CaptureInBoundaryInteraction::CaptureInBoundaryInteraction() :
-    capture_range_(5.0), boundary_id_(0), cool_down_period_(0.0) {
+CaptureInBoundaryInteraction::CaptureInBoundaryInteraction()
+    : capture_range_(5.0), boundary_id_(0), cool_down_period_(0.0) {
 }
 
-bool CaptureInBoundaryInteraction::init(std::map<std::string, std::string> &mission_params,
-                               std::map<std::string, std::string> &plugin_params) {
-
+bool CaptureInBoundaryInteraction::init(std::map<std::string, std::string>& mission_params,
+                                        std::map<std::string, std::string>& plugin_params) {
     capture_range_ = sc::get<double>("capture_range", plugin_params, 5.0);
     boundary_id_ = sc::get<int>("boundary_id", plugin_params, 1);
     cool_down_period_ = sc::get<double>("cool_down_period", plugin_params, 0.0);
 
-    auto callback = [&] (scrimmage::MessagePtr<sp::Shape> msg) {
+    auto callback = [&](scrimmage::MessagePtr<sp::Shape> msg) {
         if (msg->data.id().id() == boundary_id_) {
             boundary_shape_ = msg->data;
             boundary_ = sci::Boundary::make_boundary(msg->data);
@@ -89,9 +85,8 @@ bool CaptureInBoundaryInteraction::init(std::map<std::string, std::string> &miss
     return true;
 }
 
-
-bool CaptureInBoundaryInteraction::step_entity_interaction(std::list<sc::EntityPtr> &ents,
-                                                           double t, double dt) {
+bool CaptureInBoundaryInteraction::step_entity_interaction(std::list<sc::EntityPtr>& ents, double t,
+                                                           double dt) {
     if (boundary_ == nullptr) {
         return true;
     }
@@ -113,20 +108,16 @@ bool CaptureInBoundaryInteraction::step_entity_interaction(std::list<sc::EntityP
             }
         }
 
-        if (cool_down_expired &&
-            ent->id().team_id() == boundary_shape_.id().team_id() &&
-            boundary_->contains(ent->state_truth()->pos())) {
-
+        if (cool_down_expired && ent->id().team_id() == boundary_shape_.id().team_id()
+            && boundary_->contains(ent->state_truth()->pos())) {
             // Find all entities within capture range of this entity
             std::vector<ID> rtree_neighbors;
-            parent_->rtree()->neighbors_in_range(ent->state_truth()->pos(),
-                                                 rtree_neighbors,
-                                                 capture_range_,
-                                                 ent->id().id());
+            parent_->rtree()->neighbors_in_range(ent->state_truth()->pos(), rtree_neighbors,
+                                                 capture_range_, ent->id().id());
 
             // Only copy IDs whose team ID isn't the same as the boundary's
             // team ID.
-            for (ID &id : rtree_neighbors) {
+            for (ID& id : rtree_neighbors) {
                 if (id.team_id() != boundary_shape_.id().team_id()) {
                     possible_captures[id.id()] = ent->id().id();
                 }
@@ -141,8 +132,8 @@ bool CaptureInBoundaryInteraction::step_entity_interaction(std::list<sc::EntityP
         if (it != possible_captures.end()) {
             // If the entity hasn't been captured yet and it's within the
             // boundary, it is captured
-            if (already_captured_.count(it->first) == 0 &&
-                boundary_->contains(ent->state_truth()->pos())) {
+            if (already_captured_.count(it->first) == 0
+                && boundary_->contains(ent->state_truth()->pos())) {
                 ent->collision();
 
                 // Keep track of all captured entities
@@ -151,8 +142,7 @@ bool CaptureInBoundaryInteraction::step_entity_interaction(std::list<sc::EntityP
                 // Keep track of the capture time to limit the capture rate
                 prev_capture_times_[it->second] = time_->t();
 
-                auto msg =
-                    std::make_shared<sc::Message<sm::NonTeamCapture>>();
+                auto msg = std::make_shared<sc::Message<sm::NonTeamCapture>>();
                 msg->data.set_source_id(it->second);
                 msg->data.set_target_id(it->first);
                 non_team_capture_pub_->publish(msg);
@@ -163,5 +153,5 @@ bool CaptureInBoundaryInteraction::step_entity_interaction(std::list<sc::EntityP
     }
     return true;
 }
-} // namespace interaction
-} // namespace scrimmage
+}  // namespace interaction
+}  // namespace scrimmage
diff --git a/src/plugins/interaction/EnforceBoundaryInteraction/EnforceBoundaryInteraction.cpp b/src/plugins/interaction/EnforceBoundaryInteraction/EnforceBoundaryInteraction.cpp
index 1d6ca306a2..aa51616e3c 100644
--- a/src/plugins/interaction/EnforceBoundaryInteraction/EnforceBoundaryInteraction.cpp
+++ b/src/plugins/interaction/EnforceBoundaryInteraction/EnforceBoundaryInteraction.cpp
@@ -30,19 +30,17 @@
  *
  */
 
-#include <scrimmage/plugins/interaction/EnforceBoundaryInteraction/EnforceBoundaryInteraction.h>
-
-#include <scrimmage/plugin_manager/RegisterPlugin.h>
-#include <scrimmage/entity/Entity.h>
 #include <scrimmage/common/Utilities.h>
+#include <scrimmage/entity/Entity.h>
 #include <scrimmage/math/State.h>
 #include <scrimmage/parse/ParseUtils.h>
-
+#include <scrimmage/plugin_manager/RegisterPlugin.h>
 #include <scrimmage/plugins/interaction/Boundary/Boundary.h>
+#include <scrimmage/plugins/interaction/EnforceBoundaryInteraction/EnforceBoundaryInteraction.h>
 
-#include <memory>
-#include <limits>
 #include <iostream>
+#include <limits>
+#include <memory>
 
 using std::cout;
 using std::endl;
@@ -51,8 +49,7 @@ namespace sc = scrimmage;
 namespace sci = scrimmage::interaction;
 namespace sp = scrimmage_proto;
 
-REGISTER_PLUGIN(scrimmage::EntityInteraction,
-                scrimmage::interaction::EnforceBoundaryInteraction,
+REGISTER_PLUGIN(scrimmage::EntityInteraction, scrimmage::interaction::EnforceBoundaryInteraction,
                 EnforceBoundaryInteraction_plugin)
 
 namespace scrimmage {
@@ -61,19 +58,18 @@ namespace interaction {
 EnforceBoundaryInteraction::EnforceBoundaryInteraction() {
 }
 
-bool EnforceBoundaryInteraction::init(std::map<std::string, std::string> &mission_params,
-                               std::map<std::string, std::string> &plugin_params) {
-
+bool EnforceBoundaryInteraction::init(std::map<std::string, std::string>& mission_params,
+                                      std::map<std::string, std::string>& plugin_params) {
     std::vector<int> active_boundary_ids;
     if (!sc::get_vec<int>("active_boundary_ids", plugin_params, ", ", active_boundary_ids)) {
         std::cout << "Failed to parse 'active_boundary_ids'" << endl;
         return false;
     } else {
-        active_boundary_ids_ = std::set<int>(active_boundary_ids.begin(),
-                                             active_boundary_ids.end());
+        active_boundary_ids_ =
+            std::set<int>(active_boundary_ids.begin(), active_boundary_ids.end());
     }
 
-    auto callback = [&] (scrimmage::MessagePtr<sp::Shape> msg) {
+    auto callback = [&](scrimmage::MessagePtr<sp::Shape> msg) {
         if (active_boundary_ids_.count(msg->data.id().id()) != 0) {
             boundaries_.push_back(sci::Boundary::make_boundary(msg->data));
         }
@@ -83,10 +79,8 @@ bool EnforceBoundaryInteraction::init(std::map<std::string, std::string> &missio
     return true;
 }
 
-
-bool EnforceBoundaryInteraction::step_entity_interaction(std::list<sc::EntityPtr> &ents,
-                                                         double t, double dt) {
-
+bool EnforceBoundaryInteraction::step_entity_interaction(std::list<sc::EntityPtr>& ents, double t,
+                                                         double dt) {
     // If the boundaries haven't been received yet, ignore, just return.
     if (boundaries_.empty()) return true;
 
@@ -105,5 +99,5 @@ bool EnforceBoundaryInteraction::step_entity_interaction(std::list<sc::EntityPtr
     }
     return true;
 }
-} // namespace interaction
-} // namespace scrimmage
+}  // namespace interaction
+}  // namespace scrimmage
diff --git a/src/plugins/interaction/ExternalForceField/ExternalForceField.cpp b/src/plugins/interaction/ExternalForceField/ExternalForceField.cpp
index 17c6f32750..4c8ca486ac 100644
--- a/src/plugins/interaction/ExternalForceField/ExternalForceField.cpp
+++ b/src/plugins/interaction/ExternalForceField/ExternalForceField.cpp
@@ -30,31 +30,29 @@
  *
  */
 
-#include <scrimmage/plugins/interaction/ExternalForceField/ExternalForceField.h>
-
+#include <math.h>
+#include <scrimmage/common/Random.h>
+#include <scrimmage/common/Time.h>
 #include <scrimmage/common/Utilities.h>
 #include <scrimmage/entity/Entity.h>
-#include <scrimmage/plugin_manager/RegisterPlugin.h>
 #include <scrimmage/math/State.h>
-#include <scrimmage/pubsub/Message.h>
-#include <scrimmage/pubsub/Publisher.h>
 #include <scrimmage/motion/MotionModel.h>
 #include <scrimmage/parse/ParseUtils.h>
-#include <scrimmage/common/Random.h>
-#include <scrimmage/common/Time.h>
-#include <math.h>
+#include <scrimmage/plugin_manager/RegisterPlugin.h>
+#include <scrimmage/plugins/interaction/ExternalForceField/ExternalForceField.h>
+#include <scrimmage/pubsub/Message.h>
+#include <scrimmage/pubsub/Publisher.h>
 
-#include <memory>
-#include <limits>
 #include <iostream>
+#include <limits>
+#include <memory>
 
 using std::cout;
 using std::endl;
 
 namespace sc = scrimmage;
 
-REGISTER_PLUGIN(scrimmage::EntityInteraction,
-                scrimmage::interaction::ExternalForceField,
+REGISTER_PLUGIN(scrimmage::EntityInteraction, scrimmage::interaction::ExternalForceField,
                 ExternalForceField_plugin)
 
 namespace scrimmage {
@@ -63,9 +61,8 @@ namespace interaction {
 ExternalForceField::ExternalForceField() {
 }
 
-bool ExternalForceField::init(std::map<std::string, std::string> &mission_params,
-                              std::map<std::string, std::string> &plugin_params) {
-
+bool ExternalForceField::init(std::map<std::string, std::string>& mission_params,
+                              std::map<std::string, std::string>& plugin_params) {
     pub_ = advertise("GlobalNetwork", "Force_Field");
     // Parse the force type
     std::string force_type_str = sc::get("force_type", plugin_params, "constant");
@@ -75,8 +72,8 @@ bool ExternalForceField::init(std::map<std::string, std::string> &mission_params
         // Parse the constant force
         std::vector<double> force_vec;
 
-        if (str2container(sc::get("constant_force", plugin_params, "0, 0, 0"),
-                          ", ", force_vec, 3)) {
+        if (str2container(sc::get("constant_force", plugin_params, "0, 0, 0"), ", ", force_vec,
+                          3)) {
             if (force_vec[0] == 99) {
                 mag_ = sc::get("constant_mag", plugin_params, mag_);
                 ang_ = sc::get("constant_dir", plugin_params, ang_);
@@ -86,7 +83,8 @@ bool ExternalForceField::init(std::map<std::string, std::string> &mission_params
                 force_vec[1] = mag_ * sin(ang_);
                 force_vec[2] = 0;
             }
-            // std::cout << "Force is [0]: " << force_vec[0] << ", [1]: " << force_vec[1] << std::endl;
+            // std::cout << "Force is [0]: " << force_vec[0] << ", [1]: " << force_vec[1] <<
+            // std::endl;
             force_ << force_vec[0], force_vec[1], force_vec[2];
         }
     } else if (force_type_str == "variable") {
@@ -94,11 +92,10 @@ bool ExternalForceField::init(std::map<std::string, std::string> &mission_params
 
         // Parse the force change period
         std::vector<double> force_change_period_vec;
-        if (str2container(sc::get("force_change_period", plugin_params, "0.0, 1.0"),
-                          ", ", force_change_period_vec, 2)) {
-            force_change_period_noise_ =
-                parent_->random()->make_rng_normal(force_change_period_vec[0],
-                                                   force_change_period_vec[1]);
+        if (str2container(sc::get("force_change_period", plugin_params, "0.0, 1.0"), ", ",
+                          force_change_period_vec, 2)) {
+            force_change_period_noise_ = parent_->random()->make_rng_normal(
+                force_change_period_vec[0], force_change_period_vec[1]);
         } else {
             cout << "Failed to parse force_change_period" << endl;
             return false;
@@ -107,12 +104,11 @@ bool ExternalForceField::init(std::map<std::string, std::string> &mission_params
         // Parse the variable force magnitudes
         for (int i = 0; i < 3; i++) {
             std::vector<double> var_force_vec;
-            if (str2container(sc::get("variable_force_" + std::to_string(i),
-                                      plugin_params, "0.0, 1.0"),
-                              ", ", var_force_vec, 2)) {
+            if (str2container(
+                    sc::get("variable_force_" + std::to_string(i), plugin_params, "0.0, 1.0"), ", ",
+                    var_force_vec, 2)) {
                 auto force_noise =
-                    parent_->random()->make_rng_normal(var_force_vec[0],
-                                                       var_force_vec[1]);
+                    parent_->random()->make_rng_normal(var_force_vec[0], var_force_vec[1]);
                 force_noise_.push_back(force_noise);
             } else {
                 cout << "Failed to parse variable_force_" << i << endl;
@@ -141,8 +137,8 @@ bool ExternalForceField::init(std::map<std::string, std::string> &mission_params
     return true;
 }
 
-bool ExternalForceField::step_entity_interaction(std::list<sc::EntityPtr> &ents,
-                                                  double t, double dt) {
+bool ExternalForceField::step_entity_interaction(std::list<sc::EntityPtr>& ents, double t,
+                                                 double dt) {
     if (force_type_ == Variable) {
         if (time_->t() >= next_sample_time_) {
             sample_force();
@@ -157,14 +153,14 @@ bool ExternalForceField::step_entity_interaction(std::list<sc::EntityPtr> &ents,
     moment_(1) = pitch_amp_ * sin(time_->t() * 2 * M_PI * 1.0 / pitch_period_);
     moment_(2) = yaw_amp_ * sin(time_->t() * 2 * M_PI * 1.0 / yaw_period_);
 
-    for (auto &ent : ents) {
+    for (auto& ent : ents) {
         ent->motion()->set_external_force(force_);
 
         // Only apply moment force within z-boundary. Scale magnitude.
-        if (ent->state_truth()->pos()(2) >= moment_enable_min_z_ &&
-            ent->state_truth()->pos()(2) <= moment_enable_max_z_) {
-            double scale = (ent->state_truth()->pos()(2) - moment_enable_min_z_) /
-                (moment_enable_max_z_ - moment_enable_min_z_);
+        if (ent->state_truth()->pos()(2) >= moment_enable_min_z_
+            && ent->state_truth()->pos()(2) <= moment_enable_max_z_) {
+            double scale = (ent->state_truth()->pos()(2) - moment_enable_min_z_)
+                           / (moment_enable_max_z_ - moment_enable_min_z_);
             ent->motion()->set_external_moment(moment_ * scale);
         }
     }
@@ -182,5 +178,5 @@ void ExternalForceField::sample_force() {
     // Determine when the next sample time will take place
     next_sample_time_ = time_->t() + (*force_change_period_noise_)(*gener);
 }
-} // namespace interaction
-} // namespace scrimmage
+}  // namespace interaction
+}  // namespace scrimmage
diff --git a/src/plugins/interaction/FlagCaptureInteraction/FlagCaptureInteraction.cpp b/src/plugins/interaction/FlagCaptureInteraction/FlagCaptureInteraction.cpp
index 641098bb1d..4f8e17350b 100644
--- a/src/plugins/interaction/FlagCaptureInteraction/FlagCaptureInteraction.cpp
+++ b/src/plugins/interaction/FlagCaptureInteraction/FlagCaptureInteraction.cpp
@@ -30,24 +30,21 @@
  *
  */
 
-#include <scrimmage/plugins/interaction/FlagCaptureInteraction/FlagCaptureInteraction.h>
-
-#include <scrimmage/plugin_manager/RegisterPlugin.h>
-#include <scrimmage/entity/Entity.h>
 #include <scrimmage/common/Utilities.h>
+#include <scrimmage/entity/Entity.h>
 #include <scrimmage/math/State.h>
+#include <scrimmage/msgs/Capture.pb.h>
 #include <scrimmage/parse/ParseUtils.h>
-
+#include <scrimmage/plugin_manager/RegisterPlugin.h>
+#include <scrimmage/plugins/interaction/Boundary/Boundary.h>
+#include <scrimmage/plugins/interaction/FlagCaptureInteraction/FlagCaptureInteraction.h>
 #include <scrimmage/pubsub/Message.h>
 #include <scrimmage/pubsub/Publisher.h>
 #include <scrimmage/pubsub/Subscriber.h>
-#include <scrimmage/msgs/Capture.pb.h>
 
-#include <scrimmage/plugins/interaction/Boundary/Boundary.h>
-
-#include <memory>
-#include <limits>
 #include <iostream>
+#include <limits>
+#include <memory>
 
 using std::cout;
 using std::endl;
@@ -55,8 +52,7 @@ using std::endl;
 namespace sc = scrimmage;
 namespace sm = scrimmage_msgs;
 
-REGISTER_PLUGIN(scrimmage::EntityInteraction,
-                scrimmage::interaction::FlagCaptureInteraction,
+REGISTER_PLUGIN(scrimmage::EntityInteraction, scrimmage::interaction::FlagCaptureInteraction,
                 FlagCaptureInteraction_plugin)
 
 namespace scrimmage {
@@ -65,12 +61,12 @@ namespace interaction {
 FlagCaptureInteraction::FlagCaptureInteraction() {
 }
 
-bool FlagCaptureInteraction::init(std::map<std::string, std::string> &mission_params,
-                                  std::map<std::string, std::string> &plugin_params) {
+bool FlagCaptureInteraction::init(std::map<std::string, std::string>& mission_params,
+                                  std::map<std::string, std::string>& plugin_params) {
     flag_boundary_id_ = sc::get<int>("flag_boundary_id", plugin_params, 2);
     capture_boundary_id_ = sc::get<int>("capture_boundary_id", plugin_params, 1);
 
-    auto callback = [&] (scrimmage::MessagePtr<sp::Shape> msg) {
+    auto callback = [&](scrimmage::MessagePtr<sp::Shape> msg) {
         if (msg->data.id().id() == flag_boundary_id_) {
             flag_boundary_shape_ = msg->data;
             flag_boundary_ = sci::Boundary::make_boundary(msg->data);
@@ -87,9 +83,8 @@ bool FlagCaptureInteraction::init(std::map<std::string, std::string> &mission_pa
     return true;
 }
 
-bool FlagCaptureInteraction::step_entity_interaction(std::list<sc::EntityPtr> &ents,
-                                                     double t, double dt) {
-
+bool FlagCaptureInteraction::step_entity_interaction(std::list<sc::EntityPtr>& ents, double t,
+                                                     double dt) {
     if (flag_boundary_ == nullptr || capture_boundary_ == nullptr) {
         return true;
     }
@@ -98,13 +93,12 @@ bool FlagCaptureInteraction::step_entity_interaction(std::list<sc::EntityPtr> &e
     // opposing team is within the flag's boundary
     if (!flag_taken_) {
         for (sc::EntityPtr ent : ents) {
-            if (ent->id().team_id() != flag_boundary_shape_.id().team_id() &&
-                flag_boundary_->contains(ent->state_truth()->pos())) {
+            if (ent->id().team_id() != flag_boundary_shape_.id().team_id()
+                && flag_boundary_->contains(ent->state_truth()->pos())) {
                 flag_taken_ = true;
                 entity_with_flag_ = ent->id();
 
-                auto msg =
-                    std::make_shared<sc::Message<sm::FlagTaken>>();
+                auto msg = std::make_shared<sc::Message<sm::FlagTaken>>();
                 msg->data.set_entity_id(ent->id().id());
                 msg->data.set_entity_team_id(ent->id().team_id());
                 msg->data.set_flag_boundary_id(flag_boundary_shape_.id().id());
@@ -116,13 +110,12 @@ bool FlagCaptureInteraction::step_entity_interaction(std::list<sc::EntityPtr> &e
         }
     } else if (!flag_captured_) {
         auto it = id_to_ent_map_->find(entity_with_flag_.id());
-        if (it != id_to_ent_map_->end() &&
-            it->second != nullptr && it->second->state_truth() != nullptr) {
+        if (it != id_to_ent_map_->end() && it->second != nullptr
+            && it->second->state_truth() != nullptr) {
             if (capture_boundary_->contains(it->second->state_truth()->pos())) {
                 flag_captured_ = true;
 
-                auto msg =
-                    std::make_shared<sc::Message<sm::FlagCaptured>>();
+                auto msg = std::make_shared<sc::Message<sm::FlagCaptured>>();
                 msg->data.set_entity_id(entity_with_flag_.id());
                 msg->data.set_entity_team_id(entity_with_flag_.team_id());
                 msg->data.set_flag_boundary_id(flag_boundary_shape_.id().id());
@@ -137,5 +130,5 @@ bool FlagCaptureInteraction::step_entity_interaction(std::list<sc::EntityPtr> &e
     }
     return true;
 }
-} // namespace interaction
-} // namespace scrimmage
+}  // namespace interaction
+}  // namespace scrimmage
diff --git a/src/plugins/interaction/GRPCCommandString/GRPCCommandString.cpp b/src/plugins/interaction/GRPCCommandString/GRPCCommandString.cpp
index 28def08fd3..631a4629cb 100644
--- a/src/plugins/interaction/GRPCCommandString/GRPCCommandString.cpp
+++ b/src/plugins/interaction/GRPCCommandString/GRPCCommandString.cpp
@@ -30,28 +30,25 @@
  *
  */
 #include <grpc++/grpc++.h>
-
-#include <scrimmage/plugins/interaction/GRPCCommandString/GRPCCommandString.h>
-#include <scrimmage/plugins/interaction/GRPCCommandString/ScrimmageMsgServiceImpl.h>
-
-#include <scrimmage/plugin_manager/RegisterPlugin.h>
-#include <scrimmage/entity/Entity.h>
 #include <scrimmage/common/Utilities.h>
+#include <scrimmage/entity/Entity.h>
 #include <scrimmage/math/State.h>
 #include <scrimmage/parse/ParseUtils.h>
+#include <scrimmage/plugin_manager/RegisterPlugin.h>
+#include <scrimmage/plugins/interaction/GRPCCommandString/GRPCCommandString.h>
+#include <scrimmage/plugins/interaction/GRPCCommandString/ScrimmageMsgServiceImpl.h>
 #include <scrimmage/pubsub/Publisher.h>
 
-#include <memory>
-#include <limits>
 #include <iostream>
+#include <limits>
+#include <memory>
 
 using std::cout;
 using std::endl;
 
 namespace sc = scrimmage;
 
-REGISTER_PLUGIN(scrimmage::EntityInteraction,
-                scrimmage::interaction::GRPCCommandString,
+REGISTER_PLUGIN(scrimmage::EntityInteraction, scrimmage::interaction::GRPCCommandString,
                 GRPCCommandString_plugin)
 
 namespace scrimmage {
@@ -60,8 +57,8 @@ namespace interaction {
 GRPCCommandString::GRPCCommandString() {
 }
 
-bool GRPCCommandString::init(std::map<std::string, std::string> &mission_params,
-                             std::map<std::string, std::string> &plugin_params) {
+bool GRPCCommandString::init(std::map<std::string, std::string>& mission_params,
+                             std::map<std::string, std::string>& plugin_params) {
     ip_ = sc::get<std::string>("ip", plugin_params, ip_);
     port_ = sc::get<int>("port", plugin_params, port_);
 
@@ -70,9 +67,8 @@ bool GRPCCommandString::init(std::map<std::string, std::string> &mission_params,
     return true;
 }
 
-
-bool GRPCCommandString::step_entity_interaction(std::list<sc::EntityPtr> &ents,
-                                                  double t, double dt) {
+bool GRPCCommandString::step_entity_interaction(std::list<sc::EntityPtr>& ents, double t,
+                                                double dt) {
     // Check for new messages:
     msgs_mutex_.lock();
     while (msgs_.size() > 0) {
@@ -110,14 +106,14 @@ void GRPCCommandString::run_server() {
     builder.RegisterService(&service);
     std::unique_ptr<grpc::Server> server(builder.BuildAndStart());
     std::cout << "GRPCCommandString listening on " << result << std::endl;
-    server->Wait(); // this function blocks (should be in thread now)
+    server->Wait();  // this function blocks (should be in thread now)
 }
 
-void GRPCCommandString::push_msg(const scrimmage_msgs::CommandString &msg) {
+void GRPCCommandString::push_msg(const scrimmage_msgs::CommandString& msg) {
     msgs_mutex_.lock();
     msgs_.push(msg);
     msgs_mutex_.unlock();
 }
 
-} // namespace interaction
-} // namespace scrimmage
+}  // namespace interaction
+}  // namespace scrimmage
diff --git a/src/plugins/interaction/GRPCCommandString/ScrimmageMsgServiceImpl.cpp b/src/plugins/interaction/GRPCCommandString/ScrimmageMsgServiceImpl.cpp
index 62a54825aa..5e39461083 100644
--- a/src/plugins/interaction/GRPCCommandString/ScrimmageMsgServiceImpl.cpp
+++ b/src/plugins/interaction/GRPCCommandString/ScrimmageMsgServiceImpl.cpp
@@ -30,16 +30,15 @@
  *
  */
 
-#include <scrimmage/plugins/interaction/GRPCCommandString/ScrimmageMsgServiceImpl.h>
-#include <scrimmage/plugins/interaction/GRPCCommandString/GRPCCommandString.h>
 #include <scrimmage/msgs/Command.grpc.pb.h>
+#include <scrimmage/plugins/interaction/GRPCCommandString/GRPCCommandString.h>
+#include <scrimmage/plugins/interaction/GRPCCommandString/ScrimmageMsgServiceImpl.h>
 
 #include <list>
 
 namespace scrimmage {
-ScrimmageMsgServiceImpl::ScrimmageMsgServiceImpl(
-    std::shared_ptr<Plugin> plugin) :
-    plugin_(std::dynamic_pointer_cast<interaction::GRPCCommandString>(plugin)) {
+ScrimmageMsgServiceImpl::ScrimmageMsgServiceImpl(std::shared_ptr<Plugin> plugin)
+    : plugin_(std::dynamic_pointer_cast<interaction::GRPCCommandString>(plugin)) {
 }
 
 grpc::Status scrimmage::ScrimmageMsgServiceImpl::SendCommandString(
@@ -50,4 +49,4 @@ grpc::Status scrimmage::ScrimmageMsgServiceImpl::SendCommandString(
     return grpc::Status::OK;
 }
 
-} // namespace scrimmage
+}  // namespace scrimmage
diff --git a/src/plugins/interaction/GraphInteraction/GraphInteraction.cpp b/src/plugins/interaction/GraphInteraction/GraphInteraction.cpp
index ca08043615..9e0057fa08 100644
--- a/src/plugins/interaction/GraphInteraction/GraphInteraction.cpp
+++ b/src/plugins/interaction/GraphInteraction/GraphInteraction.cpp
@@ -30,48 +30,43 @@
  *
  */
 
-#include <scrimmage/plugins/interaction/GraphInteraction/GraphUtils.h>
-#include <scrimmage/plugins/interaction/GraphInteraction/GraphInteraction.h>
-
-#include <scrimmage/common/Utilities.h>
 #include <scrimmage/common/FileSearch.h>
+#include <scrimmage/common/Utilities.h>
 #include <scrimmage/entity/Entity.h>
-#include <scrimmage/plugin_manager/RegisterPlugin.h>
 #include <scrimmage/math/State.h>
-#include <scrimmage/pubsub/Message.h>
-#include <scrimmage/pubsub/Publisher.h>
+#include <scrimmage/msgs/Graph.pb.h>
 #include <scrimmage/parse/MissionParse.h>
 #include <scrimmage/parse/ParseUtils.h>
-#include <scrimmage/proto/Shape.pb.h>
+#include <scrimmage/plugin_manager/RegisterPlugin.h>
+#include <scrimmage/plugins/interaction/GraphInteraction/GraphInteraction.h>
+#include <scrimmage/plugins/interaction/GraphInteraction/GraphUtils.h>
 #include <scrimmage/proto/ProtoConversions.h>
-#include <scrimmage/msgs/Graph.pb.h>
-
-#include <memory>
-#include <limits>
-#include <iostream>
-#include <fstream>
+#include <scrimmage/proto/Shape.pb.h>
+#include <scrimmage/pubsub/Message.h>
+#include <scrimmage/pubsub/Publisher.h>
 
 #include <GeographicLib/LocalCartesian.hpp>
-
 #include <boost/algorithm/string.hpp>
 #include <boost/filesystem.hpp>
 #include <boost/filesystem/path.hpp>
-#include <boost/optional.hpp>
 #include <boost/graph/adjacency_list.hpp>
 #include <boost/graph/graph_utility.hpp>
 #include <boost/graph/graphml.hpp>
+#include <boost/optional.hpp>
+#include <fstream>
+#include <iostream>
+#include <limits>
+#include <memory>
 
 using std::cout;
 using std::endl;
 using std::stoi;
 
-
 namespace fs = ::boost::filesystem;
 namespace sc = scrimmage;
 namespace sm = scrimmage_msgs;
 
-REGISTER_PLUGIN(scrimmage::EntityInteraction,
-                scrimmage::interaction::GraphInteraction,
+REGISTER_PLUGIN(scrimmage::EntityInteraction, scrimmage::interaction::GraphInteraction,
                 GraphInteraction_plugin)
 
 namespace scrimmage {
@@ -81,8 +76,8 @@ GraphInteraction::GraphInteraction() {
 }
 
 // Uses graph file to draw graph
-bool GraphInteraction::init(std::map<std::string, std::string> &mission_params,
-        std::map<std::string, std::string> &plugin_params) {
+bool GraphInteraction::init(std::map<std::string, std::string>& mission_params,
+                            std::map<std::string, std::string>& plugin_params) {
     pub_graph_ = advertise("GlobalNetwork", "Graph");
     std::string default_file_name = "default";
     std::string graph_file_name =
@@ -93,20 +88,17 @@ bool GraphInteraction::init(std::map<std::string, std::string> &mission_params,
     std::map<std::string, std::string> data_params;
     if (sc::parse_autonomy_data(plugin_params, data_params)) {
         FileSearch file_search;
-        graph_file_name = sc::get<std::string>("graph_file", data_params,
-                default_file_name);
+        graph_file_name = sc::get<std::string>("graph_file", data_params, default_file_name);
         if (graph_file_name != default_file_name) {
-            std::string graph_ext =
-                fs::path(graph_file_name).extension().string();
-            file_search.find_file(graph_file_name, graph_ext,
-                    "SCRIMMAGE_DATA_PATH", graph_file_name);
+            std::string graph_ext = fs::path(graph_file_name).extension().string();
+            file_search.find_file(graph_file_name, graph_ext, "SCRIMMAGE_DATA_PATH",
+                                  graph_file_name);
         }
     }
 
     vis_graph_ = sc::get<bool>("visualize_graph", plugin_params, true);
     id_ = sc::get<int>("id", plugin_params, 1);
-    if (graph_file_name == default_file_name)
-        return true;
+    if (graph_file_name == default_file_name) return true;
 
     std::ifstream graph_file(graph_file_name);
 
@@ -173,8 +165,8 @@ bool GraphInteraction::init(std::map<std::string, std::string> &mission_params,
     return true;
 }
 
-bool GraphInteraction::step_entity_interaction(std::list<sc::EntityPtr> &ents,
-                                                  double t, double dt) {
+bool GraphInteraction::step_entity_interaction(std::list<sc::EntityPtr>& ents, double t,
+                                               double dt) {
     if (ents.empty()) {
         return true;
     }
diff --git a/src/plugins/interaction/GraphInteraction/GraphUtils.cpp b/src/plugins/interaction/GraphInteraction/GraphUtils.cpp
index d774b83136..a5fc4b4648 100644
--- a/src/plugins/interaction/GraphInteraction/GraphUtils.cpp
+++ b/src/plugins/interaction/GraphInteraction/GraphUtils.cpp
@@ -30,18 +30,16 @@
  *
  */
 
-#include <scrimmage/plugins/interaction/GraphInteraction/GraphUtils.h>
-
 #include <scrimmage/entity/EntityPlugin.h>
-#include <scrimmage/proto/Shape.pb.h>
-#include <scrimmage/proto/ProtoConversions.h>
 #include <scrimmage/msgs/Graph.pb.h>
-
-#include <iomanip>
-#include <sstream>
+#include <scrimmage/plugins/interaction/GraphInteraction/GraphUtils.h>
+#include <scrimmage/proto/ProtoConversions.h>
+#include <scrimmage/proto/Shape.pb.h>
 
 #include <boost/range/adaptor/transformed.hpp>
 #include <boost/range/algorithm/transform.hpp>
+#include <iomanip>
+#include <sstream>
 
 namespace ba = boost::adaptors;
 namespace br = boost::range;
@@ -50,28 +48,25 @@ namespace sp = scrimmage_proto;
 namespace scrimmage {
 namespace interaction {
 
-std::tuple<std::shared_ptr<scrimmage_proto::Shape>, NodePairShapeMap, NodeShapeMap>
-draw_graph(
-        scrimmage_msgs::Graph &graph,
-        const std::unordered_map<uint64_t, scrimmage_proto::Vector3d> &node_idx_to_pos,
-        DrawNodeLabels draw_node_labels,
-        EntityPluginPtr plugin) {
-
-    const std::vector<int> black {0, 0, 0};
-    const std::vector<int> white {255, 255, 255};
-    const std::vector<int> blue {0, 0, 255};
+std::tuple<std::shared_ptr<scrimmage_proto::Shape>, NodePairShapeMap, NodeShapeMap> draw_graph(
+    scrimmage_msgs::Graph& graph,
+    const std::unordered_map<uint64_t, scrimmage_proto::Vector3d>& node_idx_to_pos,
+    DrawNodeLabels draw_node_labels, EntityPluginPtr plugin) {
+    const std::vector<int> black{0, 0, 0};
+    const std::vector<int> white{255, 255, 255};
+    const std::vector<int> blue{0, 0, 255};
 
     NodePairShapeMap edge_shapes;
 
-    for (auto &e : graph.edges()) {
+    for (auto& e : graph.edges()) {
         auto edge_shape = std::make_shared<sp::Shape>();
 
         set(edge_shape->mutable_color(), black);
         edge_shape->set_opacity(1.0);
         edge_shape->set_persistent(true);
 
-        auto &p1 = node_idx_to_pos.at(e.start_node_id());
-        auto &p2 = node_idx_to_pos.at(e.end_node_id());
+        auto& p1 = node_idx_to_pos.at(e.start_node_id());
+        auto& p2 = node_idx_to_pos.at(e.end_node_id());
 
         set(edge_shape->mutable_line()->mutable_start(), p1.x(), p1.y(), p1.z());
         set(edge_shape->mutable_line()->mutable_end(), p2.x(), p2.y(), p2.z());
@@ -82,8 +77,8 @@ draw_graph(
 
     auto node_shape = std::make_shared<sp::Shape>();
     node_shape->set_persistent(true);
-    auto to_pt = [&](auto &n) {return n.point();};
-    for (const auto &pt : graph.nodes() | ba::transformed(to_pt)) {
+    auto to_pt = [&](auto& n) { return n.point(); };
+    for (const auto& pt : graph.nodes() | ba::transformed(to_pt)) {
         set(node_shape->mutable_pointcloud()->add_point(), pt.x(), pt.y(), pt.z());
         set(node_shape->mutable_pointcloud()->add_color(), blue);
     }
@@ -92,7 +87,7 @@ draw_graph(
 
     NodeShapeMap text_shapes;
 
-    for (const auto &node : graph.nodes()) {
+    for (const auto& node : graph.nodes()) {
         auto text_shape = std::make_shared<sp::Shape>();
         text_shape->set_persistent(true);
         text_shape->set_opacity(1.0);
@@ -113,12 +108,11 @@ draw_graph(
 }
 
 std::unordered_map<uint64_t, scrimmage_proto::Vector3d> nodes_idxs_to_pos_map(
-        const scrimmage_msgs::Graph &graph) {
-
-    auto to_pos = [&](auto &node) {return std::make_pair(node.id(), node.point());};
+    const scrimmage_msgs::Graph& graph) {
+    auto to_pos = [&](auto& node) { return std::make_pair(node.id(), node.point()); };
     std::unordered_map<uint64_t, scrimmage_proto::Vector3d> out;
     br::transform(graph.nodes(), std::inserter(out, out.begin()), to_pos);
     return out;
 }
-} // namespace interaction
-} // namespace scrimmage
+}  // namespace interaction
+}  // namespace scrimmage
diff --git a/src/plugins/interaction/GroundCollision/GroundCollision.cpp b/src/plugins/interaction/GroundCollision/GroundCollision.cpp
index 49f10965ca..e90270b772 100644
--- a/src/plugins/interaction/GroundCollision/GroundCollision.cpp
+++ b/src/plugins/interaction/GroundCollision/GroundCollision.cpp
@@ -30,37 +30,34 @@
  *
  */
 
-#include <scrimmage/plugin_manager/RegisterPlugin.h>
 #include <scrimmage/entity/Entity.h>
-#include <scrimmage/parse/ParseUtils.h>
-#include <scrimmage/parse/MissionParse.h>
 #include <scrimmage/math/State.h>
-#include <scrimmage/pubsub/Message.h>
-#include <scrimmage/msgs/Collision.pb.h>
 #include <scrimmage/motion/MotionModel.h>
-
+#include <scrimmage/msgs/Collision.pb.h>
+#include <scrimmage/parse/MissionParse.h>
+#include <scrimmage/parse/ParseUtils.h>
+#include <scrimmage/plugin_manager/RegisterPlugin.h>
 #include <scrimmage/plugins/interaction/GroundCollision/GroundCollision.h>
-
-#include <memory>
+#include <scrimmage/pubsub/Message.h>
 
 #include <GeographicLib/LocalCartesian.hpp>
+#include <memory>
 
 namespace sc = scrimmage;
 namespace sm = scrimmage_msgs;
 
-REGISTER_PLUGIN(scrimmage::EntityInteraction, scrimmage::interaction::GroundCollision, GroundCollision_plugin)
+REGISTER_PLUGIN(scrimmage::EntityInteraction, scrimmage::interaction::GroundCollision,
+                GroundCollision_plugin)
 
 namespace scrimmage {
 namespace interaction {
 
-GroundCollision::GroundCollision() : ground_collision_z_(0.0),
-                                     remove_on_collision_(true),
-                                     enable_startup_collisions_(true) {
+GroundCollision::GroundCollision()
+    : ground_collision_z_(0.0), remove_on_collision_(true), enable_startup_collisions_(true) {
 }
 
-bool GroundCollision::init(std::map<std::string, std::string> &mission_params,
-                           std::map<std::string, std::string> &plugin_params) {
-
+bool GroundCollision::init(std::map<std::string, std::string>& mission_params,
+                           std::map<std::string, std::string>& plugin_params) {
     remove_on_collision_ = sc::get<bool>("remove_on_collision", plugin_params, true);
     enable_startup_collisions_ = sc::get<bool>("enable_startup_collisions", plugin_params, true);
 
@@ -71,8 +68,7 @@ bool GroundCollision::init(std::map<std::string, std::string> &mission_params,
         double x, y, z, alt;
         alt = sc::get("ground_collision_altitude", plugin_params, 0.0);
         parent_->projection()->Forward(parent_->mp()->latitude_origin(),
-                                       parent_->mp()->longitude_origin(), alt,
-                                       x, y, z);
+                                       parent_->mp()->longitude_origin(), alt, x, y, z);
         ground_collision_z_ = z;
     }
 
@@ -82,8 +78,7 @@ bool GroundCollision::init(std::map<std::string, std::string> &mission_params,
     return true;
 }
 
-bool GroundCollision::step_entity_interaction(std::list<sc::EntityPtr> &ents,
-                                              double t, double dt) {
+bool GroundCollision::step_entity_interaction(std::list<sc::EntityPtr>& ents, double t, double dt) {
     // Account for entities colliding with
     for (sc::EntityPtr ent : ents) {
         if (team_ != "all" && std::stoi(team_) != ent->id().team_id()) {
@@ -112,9 +107,7 @@ bool GroundCollision::step_entity_interaction(std::list<sc::EntityPtr> &ents,
     return true;
 }
 
-bool GroundCollision::collision_exists(std::list<sc::EntityPtr> &ents,
-                                       Eigen::Vector3d &p) {
-
+bool GroundCollision::collision_exists(std::list<sc::EntityPtr>& ents, Eigen::Vector3d& p) {
     if (!enable_startup_collisions_) {
         return false;
     }
@@ -124,5 +117,5 @@ bool GroundCollision::collision_exists(std::list<sc::EntityPtr> &ents,
     }
     return false;
 }
-} // namespace interaction
-} // namespace scrimmage
+}  // namespace interaction
+}  // namespace scrimmage
diff --git a/src/plugins/interaction/MapGen2D/MapGen2D.cpp b/src/plugins/interaction/MapGen2D/MapGen2D.cpp
index 938d953563..106709a4a8 100644
--- a/src/plugins/interaction/MapGen2D/MapGen2D.cpp
+++ b/src/plugins/interaction/MapGen2D/MapGen2D.cpp
@@ -30,22 +30,21 @@
  *
  */
 
-#include <scrimmage/common/Utilities.h>
 #include <scrimmage/common/FileSearch.h>
+#include <scrimmage/common/Utilities.h>
 #include <scrimmage/entity/Entity.h>
 #include <scrimmage/math/State.h>
-#include <scrimmage/parse/ParseUtils.h>
 #include <scrimmage/parse/ConfigParse.h>
+#include <scrimmage/parse/ParseUtils.h>
 #include <scrimmage/plugin_manager/RegisterPlugin.h>
-#include <scrimmage/plugins/interaction/MapGen2D/MapGen2D.h>
 #include <scrimmage/plugins/interaction/MapGen2D/Map2DInfo.h>
+#include <scrimmage/plugins/interaction/MapGen2D/MapGen2D.h>
 #include <scrimmage/proto/ProtoConversions.h>
 #include <scrimmage/pubsub/Message.h>
 
-#include <memory>
-#include <limits>
 #include <iostream>
-
+#include <limits>
+#include <memory>
 #include <opencv2/core/core.hpp>
 #include <opencv2/highgui/highgui.hpp>
 #include <opencv2/imgproc/imgproc.hpp>
@@ -61,14 +60,12 @@ REGISTER_PLUGIN(scrimmage::EntityInteraction, scrimmage::interaction::MapGen2D,
 namespace scrimmage {
 namespace interaction {
 
-bool MapGen2D::init(std::map<std::string, std::string> &mission_params,
-                    std::map<std::string, std::string> &plugin_params) {
-
+bool MapGen2D::init(std::map<std::string, std::string>& mission_params,
+                    std::map<std::string, std::string>& plugin_params) {
     pub_shape_gen_ = advertise("GlobalNetwork", "ShapeGenerated");
     pub_map_2d_info_ = advertise("GlobalNetwork", "Map2DInfo");
 
-    show_map_debug_ = sc::get<bool>("show_map_debug", plugin_params,
-                                    false);
+    show_map_debug_ = sc::get<bool>("show_map_debug", plugin_params, false);
 
     ///////////////////////////////
     // Find the map params
@@ -77,9 +74,8 @@ bool MapGen2D::init(std::map<std::string, std::string> &mission_params,
     map_parse.set_required("resolution");
 
     sc::FileSearch file_search;
-    std::map<std::string, std::string> overrides; // empty, no overrides
-    if (!map_parse.parse(overrides, plugin_params["map"],
-                            "SCRIMMAGE_DATA_PATH", file_search)) {
+    std::map<std::string, std::string> overrides;  // empty, no overrides
+    if (!map_parse.parse(overrides, plugin_params["map"], "SCRIMMAGE_DATA_PATH", file_search)) {
         cout << "Failed to find map: " << plugin_params["map"] << endl;
         return false;
     }
@@ -87,19 +83,15 @@ bool MapGen2D::init(std::map<std::string, std::string> &mission_params,
     resolution_ = sc::get<double>("resolution", map_parse.params(), 1.0);
     wall_bottom_z_ = sc::get<double>("wall_bottom_z", map_parse.params(), 0.0);
     wall_height_ = sc::get<double>("wall_height", map_parse.params(), 5.0);
-    enable_map_boundary_ = sc::get<bool>("enable_map_boundary",
-                                         map_parse.params(), false);
-    occupied_thresh_ = sc::get<double>("occupied_thresh", map_parse.params(),
-                                       0.65);
+    enable_map_boundary_ = sc::get<bool>("enable_map_boundary", map_parse.params(), false);
+    occupied_thresh_ = sc::get<double>("occupied_thresh", map_parse.params(), 0.65);
 
     x_origin_ = sc::get<double>("x_origin", map_parse.params(), 0);
     y_origin_ = sc::get<double>("y_origin", map_parse.params(), 0);
     z_origin_ = sc::get<double>("z_origin", map_parse.params(), 0);
 
     // Parse wall_color (default to blue)
-    std::string color_str = sc::get<std::string>("wall_color",
-                                                 map_parse.params(),
-                                                 "0 0 255");
+    std::string color_str = sc::get<std::string>("wall_color", map_parse.params(), "0 0 255");
 
     // Parse wall color
     std::vector<int> color;
@@ -109,8 +101,7 @@ bool MapGen2D::init(std::map<std::string, std::string> &mission_params,
         color = {0, 0, 255};
     }
 
-    std::string filename = map_parse.params()["XML_DIR"] + "/" +
-        map_parse.params()["filename"];
+    std::string filename = map_parse.params()["XML_DIR"] + "/" + map_parse.params()["filename"];
 
     map_img_ = cv::imread(filename, cv::IMREAD_COLOR);
     if (!map_img_.data) {
@@ -128,8 +119,7 @@ bool MapGen2D::init(std::map<std::string, std::string> &mission_params,
         double height = rect.height * resolution_;
 
         // Convert rectangle into cube shape
-        Eigen::Vector3d center(x + width/2.0 + x_origin_,
-                               y - height/2.0 + y_origin_,
+        Eigen::Vector3d center(x + width / 2.0 + x_origin_, y - height / 2.0 + y_origin_,
                                wall_bottom_z_ + wall_height_ / 2.0 + z_origin_);
 
         sc::Quaternion quat(0, 0, 0);
@@ -145,7 +135,7 @@ bool MapGen2D::init(std::map<std::string, std::string> &mission_params,
         sc::set(wall->mutable_cuboid()->mutable_quat(), quat);
         draw_shape(wall);
 
-        sp::Shape *shape = msg->data.add_shape();
+        sp::Shape* shape = msg->data.add_shape();
         *shape = *wall;
     }
 
@@ -156,9 +146,7 @@ bool MapGen2D::init(std::map<std::string, std::string> &mission_params,
     return true;
 }
 
-bool MapGen2D::step_entity_interaction(std::list<sc::EntityPtr> &ents,
-                                                  double t, double dt) {
-
+bool MapGen2D::step_entity_interaction(std::list<sc::EntityPtr>& ents, double t, double dt) {
     if (!map_info_published_) {
         map_info_published_ = true;
 
@@ -174,14 +162,13 @@ bool MapGen2D::step_entity_interaction(std::list<sc::EntityPtr> &ents,
     return true;
 }
 
-std::list<cv::Rect> MapGen2D::find_rectangles(cv::Mat &img, int threshold) {
+std::list<cv::Rect> MapGen2D::find_rectangles(cv::Mat& img, int threshold) {
     // Make sure image is gray and apply threshold
     cv::Mat gray;
     cv::cvtColor(img, gray, cv::COLOR_BGRA2GRAY);
 
     cv::Mat thresh;
-    cv::threshold(gray, thresh, std::floor(threshold*255), 255,
-                  cv::THRESH_BINARY_INV);
+    cv::threshold(gray, thresh, std::floor(threshold * 255), 255, cv::THRESH_BINARY_INV);
 
     cv::Mat img_rects = img.clone();
 
@@ -224,7 +211,7 @@ std::list<cv::Rect> MapGen2D::find_rectangles(cv::Mat &img, int threshold) {
                     }
                 }
 
-                cv::Rect rect(j, i, end_c-j, end_r-i);
+                cv::Rect rect(j, i, end_c - j, end_r - i);
                 cv::rectangle(img_rects, rect, cv::Scalar(0, 0, 255), 1, 8, 0);
                 cv::Mat roi = I(rect);
                 roi.setTo(0);
@@ -235,13 +222,12 @@ std::list<cv::Rect> MapGen2D::find_rectangles(cv::Mat &img, int threshold) {
     }
 
     if (enable_map_boundary_) {
-        rects.push_back(cv::Rect(0, 0, img.cols, 1)); // top rect
-        rects.push_back(cv::Rect(0, 0, 1, img.rows)); // left rect
-        rects.push_back(cv::Rect(img.cols, 0, 1, img.rows)); // right rect
-        rects.push_back(cv::Rect(0, img.rows, img.cols, 1)); // bottom rect
+        rects.push_back(cv::Rect(0, 0, img.cols, 1));         // top rect
+        rects.push_back(cv::Rect(0, 0, 1, img.rows));         // left rect
+        rects.push_back(cv::Rect(img.cols, 0, 1, img.rows));  // right rect
+        rects.push_back(cv::Rect(0, img.rows, img.cols, 1));  // bottom rect
     }
 
-
     if (show_map_debug_) {
         cout << "Number of rectangles: " << rects.size() << endl;
         cv::imshow("Original", img);
@@ -252,5 +238,5 @@ std::list<cv::Rect> MapGen2D::find_rectangles(cv::Mat &img, int threshold) {
     }
     return rects;
 }
-} // namespace interaction
-} // namespace scrimmage
+}  // namespace interaction
+}  // namespace scrimmage
diff --git a/src/plugins/interaction/ROSClockServer/ROSClockServer.cpp b/src/plugins/interaction/ROSClockServer/ROSClockServer.cpp
index 085b034225..e755f8f2c6 100644
--- a/src/plugins/interaction/ROSClockServer/ROSClockServer.cpp
+++ b/src/plugins/interaction/ROSClockServer/ROSClockServer.cpp
@@ -30,28 +30,25 @@
  *
  */
 
-#include <scrimmage/plugins/interaction/ROSClockServer/ROSClockServer.h>
-
+#include <rosgraph_msgs/Clock.h>
+#include <scrimmage/common/Time.h>
 #include <scrimmage/common/Utilities.h>
 #include <scrimmage/entity/Entity.h>
-#include <scrimmage/plugin_manager/RegisterPlugin.h>
 #include <scrimmage/math/State.h>
-#include <scrimmage/common/Time.h>
-
-#include <rosgraph_msgs/Clock.h>
+#include <scrimmage/plugin_manager/RegisterPlugin.h>
+#include <scrimmage/plugins/interaction/ROSClockServer/ROSClockServer.h>
 
-#include <memory>
-#include <limits>
-#include <iostream>
 #include <iomanip>
+#include <iostream>
+#include <limits>
+#include <memory>
 
 using std::cout;
 using std::endl;
 
 namespace sc = scrimmage;
 
-REGISTER_PLUGIN(scrimmage::EntityInteraction,
-                scrimmage::interaction::ROSClockServer,
+REGISTER_PLUGIN(scrimmage::EntityInteraction, scrimmage::interaction::ROSClockServer,
                 ROSClockServer_plugin)
 
 namespace scrimmage {
@@ -60,12 +57,12 @@ namespace interaction {
 ROSClockServer::ROSClockServer() {
 }
 
-bool ROSClockServer::init(std::map<std::string, std::string> &mission_params,
-                          std::map<std::string, std::string> &plugin_params) {
+bool ROSClockServer::init(std::map<std::string, std::string>& mission_params,
+                          std::map<std::string, std::string>& plugin_params) {
     // Store the current UNIX time at startup. We will add the simulation time
     // (which starts at 0 seconds), to this start time when publishing to the
     // ROS clock server.
-    sim_start_time_  = std::chrono::system_clock::now();
+    sim_start_time_ = std::chrono::system_clock::now();
 
     // initialize ros
     if (!ros::isInitialized()) {
@@ -84,7 +81,7 @@ bool ROSClockServer::init(std::map<std::string, std::string> &mission_params,
 }
 
 void ROSClockServer::close(double t) {
-    (void) t;
+    (void)t;
     running_ = false;
     check_rosmaster_thread_.join();
 }
@@ -94,7 +91,7 @@ void ROSClockServer::check_rosmaster() {
     if (prev_rosmaster_state_ == false && rosmaster_state == true) {
         ros::param::set("/use_sim_time", true);
         // time to reinitialize
-        nh_ = std::make_shared<ros::NodeHandle>(); // this actually starts the node
+        nh_ = std::make_shared<ros::NodeHandle>();  // this actually starts the node
         // Create Publisher
         pub_mutex_.lock();
         clock_pub_.shutdown();
@@ -104,7 +101,6 @@ void ROSClockServer::check_rosmaster() {
     prev_rosmaster_state_ = rosmaster_state;
 }
 
-
 void ROSClockServer::check_rosmaster_loop() {
     while (running_) {
         check_rosmaster();
@@ -115,9 +111,8 @@ void ROSClockServer::check_rosmaster_loop() {
 void ROSClockServer::publish_clock_msg(const double& t) {
     // Add the current simulation time to the system time at the start of the
     // simulation.
-    std::chrono::time_point<std::chrono::system_clock,
-                            std::chrono::duration<double>> sim_time_point =
-            sim_start_time_ + std::chrono::duration<double>(t);
+    std::chrono::time_point<std::chrono::system_clock, std::chrono::duration<double>>
+        sim_time_point = sim_start_time_ + std::chrono::duration<double>(t);
 
     // Convert the time_point into a duration that represents double time since
     // the epoch.
@@ -137,10 +132,9 @@ void ROSClockServer::publish_clock_msg(const double& t) {
     pub_mutex_.unlock();
 }
 
-bool ROSClockServer::step_entity_interaction(std::list<sc::EntityPtr> &ents,
-                                             double t, double dt) {
+bool ROSClockServer::step_entity_interaction(std::list<sc::EntityPtr>& ents, double t, double dt) {
     publish_clock_msg(time_->t());
     return true;
 }
-} // namespace interaction
-} // namespace scrimmage
+}  // namespace interaction
+}  // namespace scrimmage
diff --git a/src/plugins/interaction/ROSShapeViz/ROSShapeViz.cpp b/src/plugins/interaction/ROSShapeViz/ROSShapeViz.cpp
index fe3ca4caa3..2459c63dd4 100644
--- a/src/plugins/interaction/ROSShapeViz/ROSShapeViz.cpp
+++ b/src/plugins/interaction/ROSShapeViz/ROSShapeViz.cpp
@@ -30,25 +30,23 @@
  *
  */
 
-#include <scrimmage/plugins/interaction/ROSShapeViz/ROSShapeViz.h>
-
+#include <scrimmage/common/Shape.h>
 #include <scrimmage/common/Utilities.h>
 #include <scrimmage/entity/Entity.h>
-#include <scrimmage/plugin_manager/RegisterPlugin.h>
 #include <scrimmage/math/State.h>
-#include <scrimmage/common/Shape.h>
+#include <scrimmage/plugin_manager/RegisterPlugin.h>
+#include <scrimmage/plugins/interaction/ROSShapeViz/ROSShapeViz.h>
 
-#include <memory>
-#include <limits>
 #include <iostream>
+#include <limits>
+#include <memory>
 
 using std::cout;
 using std::endl;
 
 namespace sc = scrimmage;
 
-REGISTER_PLUGIN(scrimmage::EntityInteraction,
-                scrimmage::interaction::ROSShapeViz,
+REGISTER_PLUGIN(scrimmage::EntityInteraction, scrimmage::interaction::ROSShapeViz,
                 ROSShapeViz_plugin)
 
 namespace scrimmage {
@@ -57,9 +55,8 @@ namespace interaction {
 ROSShapeViz::ROSShapeViz() {
 }
 
-bool ROSShapeViz::init(std::map<std::string, std::string> &mission_params,
-                       std::map<std::string, std::string> &plugin_params) {
-
+bool ROSShapeViz::init(std::map<std::string, std::string>& mission_params,
+                       std::map<std::string, std::string>& plugin_params) {
     // initialize ros
     if (!ros::isInitialized()) {
         int argc = 0;
@@ -72,17 +69,14 @@ bool ROSShapeViz::init(std::map<std::string, std::string> &mission_params,
 }
 
 void ROSShapeViz::shapes_cb(const visualization_msgs::Marker::ConstPtr& msg) {
-    if (msg->type == visualization_msgs::Marker::POINTS &&
-        msg->action == 0 /* ADD an object */) {
-
+    if (msg->type == visualization_msgs::Marker::POINTS && msg->action == 0 /* ADD an object */) {
         std::list<Eigen::Vector3d> points;
         for (auto& p : msg->points) {
             points.push_back(Eigen::Vector3d(p.x, p.y, p.z));
         }
 
         std::list<Eigen::Vector3d> point_colors;
-        auto shape = shape::make_pointcloud(points, point_colors, 5,
-                                            Eigen::Vector3d(255, 0, 0));
+        auto shape = shape::make_pointcloud(points, point_colors, 5, Eigen::Vector3d(255, 0, 0));
 
         // Set the ID based on the message namespace and ID
         std::string hash_str = msg->ns + std::to_string(msg->id);
@@ -91,16 +85,11 @@ void ROSShapeViz::shapes_cb(const visualization_msgs::Marker::ConstPtr& msg) {
         shape->set_hash_set(true);
 
         draw_shape(shape);
-    } else if (msg->type == visualization_msgs::Marker::SPHERE &&
-               msg->action == 0 /* ADD an object */) {
-
-        Eigen::Vector3d point(msg->pose.position.x,
-                              msg->pose.position.y,
-                              msg->pose.position.z);
+    } else if (msg->type == visualization_msgs::Marker::SPHERE
+               && msg->action == 0 /* ADD an object */) {
+        Eigen::Vector3d point(msg->pose.position.x, msg->pose.position.y, msg->pose.position.z);
 
-        auto shape = shape::make_sphere(point,
-                                        msg->scale.x,
-                                        Eigen::Vector3d(0, 0, 0));
+        auto shape = shape::make_sphere(point, msg->scale.x, Eigen::Vector3d(0, 0, 0));
         // Set the ID based on the message namespace and ID
         std::string hash_str = msg->ns + std::to_string(msg->id);
         std::size_t hash_id = std::hash<std::string>{}(hash_str);
@@ -111,10 +100,9 @@ void ROSShapeViz::shapes_cb(const visualization_msgs::Marker::ConstPtr& msg) {
     }
 }
 
-bool ROSShapeViz::step_entity_interaction(std::list<sc::EntityPtr> &ents,
-                                          double t, double dt) {
+bool ROSShapeViz::step_entity_interaction(std::list<sc::EntityPtr>& ents, double t, double dt) {
     ros::spinOnce();
     return true;
 }
-} // namespace interaction
-} // namespace scrimmage
+}  // namespace interaction
+}  // namespace scrimmage
diff --git a/src/plugins/interaction/RandomAttrit/RandomAttrit.cpp b/src/plugins/interaction/RandomAttrit/RandomAttrit.cpp
index c6c273dd79..778bc41093 100644
--- a/src/plugins/interaction/RandomAttrit/RandomAttrit.cpp
+++ b/src/plugins/interaction/RandomAttrit/RandomAttrit.cpp
@@ -29,27 +29,25 @@
  *
  */
 
-#include <scrimmage/plugins/interaction/RandomAttrit/RandomAttrit.h>
-
+#include <scrimmage/common/Random.h>
 #include <scrimmage/common/Utilities.h>
 #include <scrimmage/entity/Entity.h>
-#include <scrimmage/plugin_manager/RegisterPlugin.h>
 #include <scrimmage/math/State.h>
 #include <scrimmage/parse/ParseUtils.h>
-#include <scrimmage/common/Random.h>
+#include <scrimmage/plugin_manager/RegisterPlugin.h>
+#include <scrimmage/plugins/interaction/RandomAttrit/RandomAttrit.h>
 
-#include <memory>
-#include <limits>
-#include <iostream>
 #include <cmath>
+#include <iostream>
+#include <limits>
+#include <memory>
 
 using std::cout;
 using std::endl;
 
 namespace sc = scrimmage;
 
-REGISTER_PLUGIN(scrimmage::EntityInteraction,
-                scrimmage::interaction::RandomAttrit,
+REGISTER_PLUGIN(scrimmage::EntityInteraction, scrimmage::interaction::RandomAttrit,
                 RandomAttrit_plugin)
 
 namespace scrimmage {
@@ -58,8 +56,8 @@ namespace interaction {
 RandomAttrit::RandomAttrit() {
 }
 
-bool RandomAttrit::init(std::map<std::string, std::string> &mission_params,
-                               std::map<std::string, std::string> &plugin_params) {
+bool RandomAttrit::init(std::map<std::string, std::string>& mission_params,
+                        std::map<std::string, std::string>& plugin_params) {
     team_id_ = sc::get("team", plugin_params, -1);
     decay_method_ = sc::get("decay_method", plugin_params, "linear");
     start_wait_time_s_ = sc::get("start_after", plugin_params, 0.0);
@@ -74,9 +72,7 @@ bool RandomAttrit::init(std::map<std::string, std::string> &mission_params,
     return true;
 }
 
-
-bool RandomAttrit::step_entity_interaction(std::list<sc::EntityPtr> &ents,
-                                                  double t, double dt) {
+bool RandomAttrit::step_entity_interaction(std::list<sc::EntityPtr>& ents, double t, double dt) {
     if (ents.empty() || num_ent_ < 0.0) {
         return true;
     }
@@ -84,13 +80,14 @@ bool RandomAttrit::step_entity_interaction(std::list<sc::EntityPtr> &ents,
     total_num_entities_ = 0;
     std::vector<sc::EntityPtr> cur_ents;
     std::copy_if(ents.begin(), ents.end(), std::back_inserter(cur_ents),
-         [&](const sc::EntityPtr& ent) {
-             bool is_stay_alive = std::find(stay_alive_ids_.begin(), stay_alive_ids_.end(),
-                                            ent->id().id()) != stay_alive_ids_.end();
-             bool team_match = ent->id().team_id() == team_id_;
-             if (team_match) total_num_entities_++;
-             return !is_stay_alive && team_match;
-         });
+                 [&](const sc::EntityPtr& ent) {
+                     bool is_stay_alive =
+                         std::find(stay_alive_ids_.begin(), stay_alive_ids_.end(), ent->id().id())
+                         != stay_alive_ids_.end();
+                     bool team_match = ent->id().team_id() == team_id_;
+                     if (team_match) total_num_entities_++;
+                     return !is_stay_alive && team_match;
+                 });
 
     if (t < start_wait_time_s_) {
         return true;
@@ -105,8 +102,8 @@ bool RandomAttrit::step_entity_interaction(std::list<sc::EntityPtr> &ents,
         double slope = -start_num_ent_ / duration_s_;
         num_ent_ = slope * (t - start_wait_time_s_) + start_num_ent_;
     } else {
-        std::cout << "RandomAttrit: Decay method, " << decay_method_
-                  << ", is not implemented." << std::endl;
+        std::cout << "RandomAttrit: Decay method, " << decay_method_ << ", is not implemented."
+                  << std::endl;
     }
 
     // else if (decay_method_ == "exponential") {
@@ -114,7 +111,7 @@ bool RandomAttrit::step_entity_interaction(std::list<sc::EntityPtr> &ents,
     if (std::ceil(num_ent_) < cur_ents.size()) {
         // pick one to attrit
         int drop_id = random_.rng_uniform_int(0, cur_ents.size() - 1);
-        cur_ents[drop_id]->set_health_points(-1); // kill entity
+        cur_ents[drop_id]->set_health_points(-1);  // kill entity
 
         auto msg = std::make_shared<sc::Message<EntityPtr>>();
         msg->data = cur_ents[drop_id];
@@ -123,5 +120,5 @@ bool RandomAttrit::step_entity_interaction(std::list<sc::EntityPtr> &ents,
 
     return true;
 }
-} // namespace interaction
-} // namespace scrimmage
+}  // namespace interaction
+}  // namespace scrimmage
diff --git a/src/plugins/interaction/SimpleCapture/SimpleCapture.cpp b/src/plugins/interaction/SimpleCapture/SimpleCapture.cpp
index cf6e9d3676..edb354e063 100644
--- a/src/plugins/interaction/SimpleCapture/SimpleCapture.cpp
+++ b/src/plugins/interaction/SimpleCapture/SimpleCapture.cpp
@@ -30,47 +30,45 @@
  *
  */
 
-#include <scrimmage/plugin_manager/RegisterPlugin.h>
-#include <scrimmage/entity/Entity.h>
 #include <scrimmage/common/Utilities.h>
+#include <scrimmage/entity/Entity.h>
 #include <scrimmage/math/State.h>
 #include <scrimmage/msgs/Capture.pb.h>
 #include <scrimmage/parse/ParseUtils.h>
+#include <scrimmage/plugin_manager/RegisterPlugin.h>
 #include <scrimmage/plugins/interaction/SimpleCapture/SimpleCapture.h>
 #include <scrimmage/pubsub/Message.h>
 #include <scrimmage/pubsub/Publisher.h>
 #include <scrimmage/pubsub/Subscriber.h>
 
-#include <memory>
 #include <limits>
+#include <memory>
 
 namespace sc = scrimmage;
 namespace sm = scrimmage_msgs;
 
-REGISTER_PLUGIN(scrimmage::EntityInteraction,
-        scrimmage::interaction::SimpleCapture, SimpleCapture_plugin)
+REGISTER_PLUGIN(scrimmage::EntityInteraction, scrimmage::interaction::SimpleCapture,
+                SimpleCapture_plugin)
 
 namespace scrimmage {
 namespace interaction {
 
-SimpleCapture::SimpleCapture(): capture_range_(0),
-    enable_team_captures_(true), enable_non_team_captures_(true) {
+SimpleCapture::SimpleCapture()
+    : capture_range_(0), enable_team_captures_(true), enable_non_team_captures_(true) {
 }
-bool SimpleCapture::init(std::map<std::string, std::string> &mission_params,
-                         std::map<std::string, std::string> &plugin_params) {
+bool SimpleCapture::init(std::map<std::string, std::string>& mission_params,
+                         std::map<std::string, std::string>& plugin_params) {
     capture_range_ = sc::get("capture_range", plugin_params, 0.0);
 
-    enable_team_captures_ =
-        sc::get<bool>("enable_team_captures", plugin_params, true);
-    enable_non_team_captures_ =
-        sc::get<bool>("enable_non_team_captures", plugin_params, true);
+    enable_team_captures_ = sc::get<bool>("enable_team_captures", plugin_params, true);
+    enable_non_team_captures_ = sc::get<bool>("enable_non_team_captures", plugin_params, true);
 
     // Setup publishers
     team_capture_pub_ = advertise("GlobalNetwork", "TeamCapture");
     non_team_capture_pub_ = advertise("GlobalNetwork", "NonTeamCapture");
 
     // Setup capture callback
-    auto capture_cb = [&] (scrimmage::MessagePtr<sm::CaptureEntity> msg) {
+    auto capture_cb = [&](scrimmage::MessagePtr<sm::CaptureEntity> msg) {
         int source_id = msg->data.source_id();
         int target_id = msg->data.target_id();
 
@@ -79,26 +77,22 @@ bool SimpleCapture::init(std::map<std::string, std::string> &mission_params,
         auto capture_result = already_captured_.emplace(target_id);
 
         if (capture_result.second) {
-            sc::EntityPtr &src = (*id_to_ent_map_)[source_id];
-            sc::EntityPtr &dst = (*id_to_ent_map_)[target_id];
-            if ((src->state_truth()->pos() - dst->state_truth()->pos()).norm() <=
-                capture_range_) {
-                if (enable_team_captures_ &&
-                    src->id().team_id() == dst->id().team_id()) {
+            sc::EntityPtr& src = (*id_to_ent_map_)[source_id];
+            sc::EntityPtr& dst = (*id_to_ent_map_)[target_id];
+            if ((src->state_truth()->pos() - dst->state_truth()->pos()).norm() <= capture_range_) {
+                if (enable_team_captures_ && src->id().team_id() == dst->id().team_id()) {
                     dst->collision();
 
-                    auto msg =
-                        std::make_shared<sc::Message<sm::TeamCapture>>();
+                    auto msg = std::make_shared<sc::Message<sm::TeamCapture>>();
                     msg->data.set_source_id(src->id().id());
                     msg->data.set_target_id(dst->id().id());
                     team_capture_pub_->publish(msg);
 
-                } else if (enable_non_team_captures_ &&
-                           src->id().team_id() != dst->id().team_id()) {
+                } else if (enable_non_team_captures_
+                           && src->id().team_id() != dst->id().team_id()) {
                     dst->collision();
 
-                    auto msg =
-                        std::make_shared<sc::Message<sm::NonTeamCapture>>();
+                    auto msg = std::make_shared<sc::Message<sm::NonTeamCapture>>();
                     msg->data.set_source_id(src->id().id());
                     msg->data.set_target_id(dst->id().id());
                     non_team_capture_pub_->publish(msg);
@@ -110,8 +104,7 @@ bool SimpleCapture::init(std::map<std::string, std::string> &mission_params,
     return true;
 }
 
-bool SimpleCapture::step_entity_interaction(std::list<sc::EntityPtr> &ents,
-                                            double t, double dt) {
+bool SimpleCapture::step_entity_interaction(std::list<sc::EntityPtr>& ents, double t, double dt) {
     return true;
 }
 }  // namespace interaction
diff --git a/src/plugins/interaction/SimpleCollision/SimpleCollision.cpp b/src/plugins/interaction/SimpleCollision/SimpleCollision.cpp
index 39b23f8404..2647877fd4 100644
--- a/src/plugins/interaction/SimpleCollision/SimpleCollision.cpp
+++ b/src/plugins/interaction/SimpleCollision/SimpleCollision.cpp
@@ -30,37 +30,36 @@
  *
  */
 
-#include <scrimmage/plugin_manager/RegisterPlugin.h>
-#include <scrimmage/entity/Entity.h>
-#include <scrimmage/common/Utilities.h>
-#include <scrimmage/common/Time.h>
+#include <scrimmage/autonomy/Autonomy.h>
 #include <scrimmage/common/ParameterServer.h>
-#include <scrimmage/parse/ParseUtils.h>
+#include <scrimmage/common/RTree.h>
+#include <scrimmage/common/Time.h>
+#include <scrimmage/common/Utilities.h>
+#include <scrimmage/entity/Entity.h>
 #include <scrimmage/math/State.h>
-#include <scrimmage/pubsub/Message.h>
 #include <scrimmage/msgs/Collision.pb.h>
-#include <scrimmage/autonomy/Autonomy.h>
-#include <scrimmage/common/RTree.h>
-
+#include <scrimmage/parse/ParseUtils.h>
+#include <scrimmage/plugin_manager/RegisterPlugin.h>
 #include <scrimmage/plugins/interaction/SimpleCollision/SimpleCollision.h>
+#include <scrimmage/pubsub/Message.h>
 
 #include <limits>
 #include <memory>
 
 namespace sm = scrimmage_msgs;
 
-REGISTER_PLUGIN(scrimmage::EntityInteraction, scrimmage::interaction::SimpleCollision, SimpleCollision_plugin)
+REGISTER_PLUGIN(scrimmage::EntityInteraction, scrimmage::interaction::SimpleCollision,
+                SimpleCollision_plugin)
 
 namespace scrimmage {
 namespace interaction {
 
-bool SimpleCollision::init(std::map<std::string, std::string> &mission_params,
-                           std::map<std::string, std::string> &plugin_params) {
+bool SimpleCollision::init(std::map<std::string, std::string>& mission_params,
+                           std::map<std::string, std::string>& plugin_params) {
     collision_range_ = get("collision_range", plugin_params, 0.0);
 
     // If startup_collision_range isn't defined, default to the collision_range
-    startup_collision_range_ = get("startup_collision_range",
-                                       plugin_params, collision_range_);
+    startup_collision_range_ = get("startup_collision_range", plugin_params, collision_range_);
 
     startup_collisions_only_ = get("startup_collisions_only", plugin_params, false);
 
@@ -76,9 +75,7 @@ bool SimpleCollision::init(std::map<std::string, std::string> &mission_params,
     return true;
 }
 
-
-bool SimpleCollision::step_entity_interaction(std::list<EntityPtr> &ents,
-                                              double t, double dt) {
+bool SimpleCollision::step_entity_interaction(std::list<EntityPtr>& ents, double t, double dt) {
     if (startup_collisions_only_) {
         return true;
     }
@@ -97,9 +94,7 @@ bool SimpleCollision::step_entity_interaction(std::list<EntityPtr> &ents,
 
             double dist = (p1 - p2).norm();
             if (dist < collision_range_) {
-                if (enable_team_collisions_ &&
-                    ent1->id().team_id() == ent2->id().team_id()) {
-
+                if (enable_team_collisions_ && ent1->id().team_id() == ent2->id().team_id()) {
                     ent1->collision();
                     ent2->collision();
 
@@ -108,8 +103,8 @@ bool SimpleCollision::step_entity_interaction(std::list<EntityPtr> &ents,
                     msg->data.set_entity_id_2(ent2->id().id());
                     team_collision_pub_->publish(msg);
 
-                } else if (enable_non_team_collisions_ &&
-                           ent1->id().team_id() != ent2->id().team_id()) {
+                } else if (enable_non_team_collisions_
+                           && ent1->id().team_id() != ent2->id().team_id()) {
                     ent1->collision();
                     ent2->collision();
 
@@ -124,15 +119,13 @@ bool SimpleCollision::step_entity_interaction(std::list<EntityPtr> &ents,
     return true;
 }
 
-bool SimpleCollision::collision_exists(std::list<EntityPtr> &ents,
-                                       Eigen::Vector3d &p) {
+bool SimpleCollision::collision_exists(std::list<EntityPtr>& ents, Eigen::Vector3d& p) {
     if (ents.empty()) {
         return false;
     } else {
         if (init_alt_deconflict_) {
             for (EntityPtr ent : ents) {
-                if (std::abs(p(2) - ent->state_truth()->pos()(2)) <=
-                    startup_collision_range_) {
+                if (std::abs(p(2) - ent->state_truth()->pos()(2)) <= startup_collision_range_) {
                     return true;
                 }
             }
@@ -149,5 +142,5 @@ bool SimpleCollision::collision_exists(std::list<EntityPtr> &ents,
     }
     return false;
 }
-} // namespace interaction
-} // namespace scrimmage
+}  // namespace interaction
+}  // namespace scrimmage
diff --git a/src/plugins/interaction/TerrainGenerator/TerrainGenerator.cpp b/src/plugins/interaction/TerrainGenerator/TerrainGenerator.cpp
index 4a7c6cdd9f..bc6de7028f 100644
--- a/src/plugins/interaction/TerrainGenerator/TerrainGenerator.cpp
+++ b/src/plugins/interaction/TerrainGenerator/TerrainGenerator.cpp
@@ -30,20 +30,19 @@
  *
  */
 
-#include <scrimmage/plugins/interaction/TerrainGenerator/TerrainGenerator.h>
-
+#include <scrimmage/common/Random.h>
 #include <scrimmage/common/Utilities.h>
 #include <scrimmage/entity/Entity.h>
-#include <scrimmage/plugin_manager/RegisterPlugin.h>
 #include <scrimmage/math/State.h>
 #include <scrimmage/parse/ParseUtils.h>
-#include <scrimmage/common/Random.h>
+#include <scrimmage/plugin_manager/RegisterPlugin.h>
+#include <scrimmage/plugins/interaction/TerrainGenerator/TerrainGenerator.h>
 #include <scrimmage/proto/Shape.pb.h>
 #include <scrimmage/pubsub/Publisher.h>
 
-#include <memory>
-#include <limits>
 #include <iostream>
+#include <limits>
+#include <memory>
 
 using std::cout;
 using std::endl;
@@ -51,8 +50,7 @@ using std::endl;
 namespace sc = scrimmage;
 namespace sp = scrimmage_proto;
 
-REGISTER_PLUGIN(scrimmage::EntityInteraction,
-                scrimmage::interaction::TerrainGenerator,
+REGISTER_PLUGIN(scrimmage::EntityInteraction, scrimmage::interaction::TerrainGenerator,
                 TerrainGenerator_plugin)
 
 namespace scrimmage {
@@ -61,18 +59,19 @@ namespace interaction {
 TerrainGenerator::TerrainGenerator() {
 }
 
-bool TerrainGenerator::init(std::map<std::string, std::string> &mission_params,
-                            std::map<std::string, std::string> &plugin_params) {
+bool TerrainGenerator::init(std::map<std::string, std::string>& mission_params,
+                            std::map<std::string, std::string>& plugin_params) {
     double x_length = sc::get<double>("x_length", plugin_params, 500.0);
     double y_length = sc::get<double>("y_length", plugin_params, 500.0);
     double x_resolution = sc::get<double>("x_resolution", plugin_params, 1.0);
     double y_resolution = sc::get<double>("y_resolution", plugin_params, 1.0);
-    double z_min = sc::get<double>("z_min", plugin_params, -std::numeric_limits<double>::infinity());
-    double z_max = sc::get<double>("z_max", plugin_params, +std::numeric_limits<double>::infinity());
+    double z_min =
+        sc::get<double>("z_min", plugin_params, -std::numeric_limits<double>::infinity());
+    double z_max =
+        sc::get<double>("z_max", plugin_params, +std::numeric_limits<double>::infinity());
     double z_std = sc::get<double>("z_std", plugin_params, 1.0);
 
-    std::string technique_str = sc::get("technique", plugin_params,
-                                        "RANDOM_WALK");
+    std::string technique_str = sc::get("technique", plugin_params, "RANDOM_WALK");
     TerrainMap::Technique technique;
     if (technique_str == "RANDOM_WALK") {
         technique = TerrainMap::Technique::RANDOM_WALK;
@@ -98,32 +97,29 @@ bool TerrainGenerator::init(std::map<std::string, std::string> &mission_params,
 
     std::vector<double> center_vec;
     Eigen::Vector3d center_point(0, 0, 0);
-    if (str2container(sc::get<std::string>("center", plugin_params, "0, 0, 0"),
-                      ", ", center_vec, 3)) {
+    if (str2container(sc::get<std::string>("center", plugin_params, "0, 0, 0"), ", ", center_vec,
+                      3)) {
         center_point = vec2eigen(center_vec);
     }
 
     std::vector<double> color_vec;
     Eigen::Vector3d color(0, 0, 0);
-    if (str2container(sc::get<std::string>("color", plugin_params, "0, 255, 0"),
-                      ", ", color_vec, 3)) {
+    if (str2container(sc::get<std::string>("color", plugin_params, "0, 255, 0"), ", ", color_vec,
+                      3)) {
         color = vec2eigen(color_vec);
     }
 
-    map_ = TerrainMap(random_->make_rng_normal(0.0, z_std),
-                      random_->gener(), technique,
-                      center_point,
-                      x_length, y_length, x_resolution, y_resolution,
-                      z_min, z_max, color);
+    map_ =
+        TerrainMap(random_->make_rng_normal(0.0, z_std), random_->gener(), technique, center_point,
+                   x_length, y_length, x_resolution, y_resolution, z_min, z_max, color);
 
     terrain_pub_ = advertise("GlobalNetwork", "Terrain");
 
     return true;
 }
 
-
-bool TerrainGenerator::step_entity_interaction(std::list<sc::EntityPtr> &ents,
-                                               double t, double dt) {
+bool TerrainGenerator::step_entity_interaction(std::list<sc::EntityPtr>& ents, double t,
+                                               double dt) {
     if (not terrain_published_) {
         terrain_published_ = true;
 
@@ -137,5 +133,5 @@ bool TerrainGenerator::step_entity_interaction(std::list<sc::EntityPtr> &ents,
     }
     return true;
 }
-} // namespace interaction
-} // namespace scrimmage
+}  // namespace interaction
+}  // namespace scrimmage
diff --git a/src/plugins/interaction/TerrainGenerator/TerrainMap.cpp b/src/plugins/interaction/TerrainGenerator/TerrainMap.cpp
index 9fbe2c369c..6b47960879 100644
--- a/src/plugins/interaction/TerrainGenerator/TerrainMap.cpp
+++ b/src/plugins/interaction/TerrainGenerator/TerrainMap.cpp
@@ -46,36 +46,45 @@ using boost::algorithm::clamp;
 namespace scrimmage {
 namespace interaction {
 
-TerrainMap::TerrainMap() {}
+TerrainMap::TerrainMap() {
+}
 
 TerrainMap::TerrainMap(std::shared_ptr<std::normal_distribution<double>> rng,
                        std::shared_ptr<std::default_random_engine> gener,
-                       const Technique &technique,
-                       const Eigen::Vector3d &center,
-                       const double &x_length, const double &y_length,
-                       const double &x_resolution, const double &y_resolution,
-                       const double &z_min, const double &z_max,
-                       const Eigen::Vector3d &color) :
-        rng_(rng), gener_(gener), technique_(technique), center_(center),
-        x_length_(x_length),
-        y_length_(y_length),
-        x_resolution_(x_resolution),
-        y_resolution_(y_resolution), z_min_(z_min), z_max_(z_max),
-        color_(color),
-        num_x_cols_(x_length_ / x_resolution_),
-        num_y_rows_(y_length_ / y_resolution_),
-        grid_(std::vector<std::vector<Node>>(num_y_rows_, std::vector<Node>(num_x_cols_))) {
+                       const Technique& technique, const Eigen::Vector3d& center,
+                       const double& x_length, const double& y_length, const double& x_resolution,
+                       const double& y_resolution, const double& z_min, const double& z_max,
+                       const Eigen::Vector3d& color)
+    : rng_(rng),
+      gener_(gener),
+      technique_(technique),
+      center_(center),
+      x_length_(x_length),
+      y_length_(y_length),
+      x_resolution_(x_resolution),
+      y_resolution_(y_resolution),
+      z_min_(z_min),
+      z_max_(z_max),
+      color_(color),
+      num_x_cols_(x_length_ / x_resolution_),
+      num_y_rows_(y_length_ / y_resolution_),
+      grid_(std::vector<std::vector<Node>>(num_y_rows_, std::vector<Node>(num_x_cols_))) {
     generate();
 }
 
-TerrainMap::TerrainMap(const scrimmage_msgs::Terrain &terrain) :
-        rng_(nullptr), gener_(nullptr), center_(Eigen::Vector3d(0, 0, 0)),
-        x_length_(terrain.x_length()), y_length_(terrain.y_length()),
-        x_resolution_(terrain.x_resolution()),
-        y_resolution_(terrain.y_resolution()), z_min_(terrain.z_min()),
-        z_max_(terrain.z_max()), num_x_cols_(x_length_ / x_resolution_),
-        num_y_rows_(y_length_ / y_resolution_),
-        grid_(std::vector<std::vector<Node>>(num_y_rows_, std::vector<Node>(num_x_cols_))) {
+TerrainMap::TerrainMap(const scrimmage_msgs::Terrain& terrain)
+    : rng_(nullptr),
+      gener_(nullptr),
+      center_(Eigen::Vector3d(0, 0, 0)),
+      x_length_(terrain.x_length()),
+      y_length_(terrain.y_length()),
+      x_resolution_(terrain.x_resolution()),
+      y_resolution_(terrain.y_resolution()),
+      z_min_(terrain.z_min()),
+      z_max_(terrain.z_max()),
+      num_x_cols_(x_length_ / x_resolution_),
+      num_y_rows_(y_length_ / y_resolution_),
+      grid_(std::vector<std::vector<Node>>(num_y_rows_, std::vector<Node>(num_x_cols_))) {
     sc::set(center_, terrain.center());
 
     // Populate the grid
@@ -155,7 +164,7 @@ bool TerrainMap::generate_linear_walk() {
     }
     center_height_adjust();
     clamp_height();
-     // Force the altitude center to be at the midpoint between z_max and z_min
+    // Force the altitude center to be at the midpoint between z_max and z_min
     center_(2) = (z_max_ + z_min_) / 2.0;
 
     // Initialize the map, such that row 0 is at z_min and the last row is at
@@ -176,7 +185,8 @@ bool TerrainMap::generate_linear_walk() {
 
 void TerrainMap::center_height_adjust() {
     // Make sure the grid's center is located at the appropriate height.
-    double offset = center_(2) - grid_[std::round(num_y_rows_/2.0)][std::round(num_x_cols_/2.0)].height;
+    double offset =
+        center_(2) - grid_[std::round(num_y_rows_ / 2.0)][std::round(num_x_cols_ / 2.0)].height;
     for (unsigned int row = 0; row < num_y_rows_; ++row) {
         for (unsigned int col = 0; col < num_x_cols_; ++col) {
             grid_[row][col].height += offset;
@@ -193,20 +203,20 @@ void TerrainMap::clamp_height() {
     }
 }
 
-double TerrainMap::get_neighbor_avg(const int &row, const int &col) {
+double TerrainMap::get_neighbor_avg(const int& row, const int& col) {
     unsigned int neighbors = 0;
     double height_sum = 0;
 
     if (row > 0) {
-        height_sum += grid_[row-1][col].height;
+        height_sum += grid_[row - 1][col].height;
         ++neighbors;
     }
     if (col > 0) {
-        height_sum += grid_[row][col-1].height;
+        height_sum += grid_[row][col - 1].height;
         ++neighbors;
     }
     if (row > 0 && col > 0) {
-        height_sum += grid_[row-1][col-1].height;
+        height_sum += grid_[row - 1][col - 1].height;
         ++neighbors;
     }
 
@@ -221,10 +231,10 @@ scrimmage::ShapePtr TerrainMap::shape() {
     shape->mutable_pointcloud()->set_size(3);
 
     for (unsigned int row = 0; row < num_y_rows_; ++row) {
-        double y = center_(1)-y_length_ / 2.0 + row * y_resolution_;
+        double y = center_(1) - y_length_ / 2.0 + row * y_resolution_;
         for (unsigned int col = 0; col < num_x_cols_; ++col) {
-            sp::Vector3d *p = shape->mutable_pointcloud()->add_point();
-            double x = center_(0)-x_length_ / 2.0 + col * x_resolution_;
+            sp::Vector3d* p = shape->mutable_pointcloud()->add_point();
+            double x = center_(0) - x_length_ / 2.0 + col * x_resolution_;
             sc::set(p, x, y, grid_[row][col].height);
         }
     }
@@ -243,7 +253,7 @@ scrimmage_msgs::Terrain TerrainMap::proto() {
     terrain.set_z_max(z_max_);
 
     for (unsigned int r = 0; r < num_y_rows_; ++r) {
-        scrimmage_msgs::Array1D *row = terrain.mutable_map()->add_row();
+        scrimmage_msgs::Array1D* row = terrain.mutable_map()->add_row();
         for (unsigned int c = 0; c < num_x_cols_; ++c) {
             row->add_col(grid_[r][c].height);
         }
@@ -251,16 +261,15 @@ scrimmage_msgs::Terrain TerrainMap::proto() {
     return terrain;
 }
 
-boost::optional<double> TerrainMap::height_at(const double &x, const double &y) {
-    int row = std::floor((y + y_length_/2.0 - center_(1)) / y_resolution_);
-    int col = std::floor((x + x_length_/2.0 - center_(0)) / x_resolution_);
-    if (row < 0 || row >= static_cast<int>(num_y_rows_) ||
-        col < 0 || col >= static_cast<int>(num_x_cols_) ||
-        not grid_[row][col].is_set) {
+boost::optional<double> TerrainMap::height_at(const double& x, const double& y) {
+    int row = std::floor((y + y_length_ / 2.0 - center_(1)) / y_resolution_);
+    int col = std::floor((x + x_length_ / 2.0 - center_(0)) / x_resolution_);
+    if (row < 0 || row >= static_cast<int>(num_y_rows_) || col < 0
+        || col >= static_cast<int>(num_x_cols_) || not grid_[row][col].is_set) {
         return boost::optional<double>{};
     }
     return grid_[row][col].height;
 }
 
-} // namespace interaction
-} // namespace scrimmage
+}  // namespace interaction
+}  // namespace scrimmage
diff --git a/src/plugins/metrics/CPA/CPA.cpp b/src/plugins/metrics/CPA/CPA.cpp
index d3abd4f2c0..93d682b39e 100644
--- a/src/plugins/metrics/CPA/CPA.cpp
+++ b/src/plugins/metrics/CPA/CPA.cpp
@@ -30,20 +30,19 @@
  *
  */
 
-#include <scrimmage/plugins/metrics/CPA/CPA.h>
-
-#include <scrimmage/plugin_manager/RegisterPlugin.h>
+#include <scrimmage/common/Utilities.h>
 #include <scrimmage/entity/Entity.h>
 #include <scrimmage/math/State.h>
-#include <scrimmage/parse/ParseUtils.h>
-#include <scrimmage/parse/MissionParse.h>
-#include <scrimmage/parse/ConfigParse.h>
-#include <scrimmage/common/Utilities.h>
 #include <scrimmage/metrics/Metrics.h>
-#include <scrimmage/pubsub/Message.h>
-#include <scrimmage/pubsub/Subscriber.h>
 #include <scrimmage/msgs/Collision.pb.h>
 #include <scrimmage/msgs/Event.pb.h>
+#include <scrimmage/parse/ConfigParse.h>
+#include <scrimmage/parse/MissionParse.h>
+#include <scrimmage/parse/ParseUtils.h>
+#include <scrimmage/plugin_manager/RegisterPlugin.h>
+#include <scrimmage/plugins/metrics/CPA/CPA.h>
+#include <scrimmage/pubsub/Message.h>
+#include <scrimmage/pubsub/Subscriber.h>
 
 #include <iostream>
 #include <limits>
@@ -54,9 +53,7 @@ using std::endl;
 namespace sc = scrimmage;
 namespace sm = scrimmage_msgs;
 
-REGISTER_PLUGIN(scrimmage::Metrics,
-                scrimmage::metrics::CPA,
-                CPA_plugin)
+REGISTER_PLUGIN(scrimmage::Metrics, scrimmage::metrics::CPA, CPA_plugin)
 
 namespace scrimmage {
 namespace metrics {
@@ -64,9 +61,10 @@ namespace metrics {
 CPA::CPA() {
 }
 
-void CPA::init(std::map<std::string, std::string> &params) {
+void CPA::init(std::map<std::string, std::string>& params) {
     team_id_ = sc::get<int>("team_id", params, team_id_);
-    // If team_id_ is set to a specific team, then team_cpa_ should be automatically set based on how the checks work below
+    // If team_id_ is set to a specific team, then team_cpa_ should be automatically set based on
+    // how the checks work below
     team_cpa_ = sc::get<bool>("team_cpa", params, team_cpa_) || (team_id_ > 0);
     min_time_s_ = sc::get<double>("min_time_s", params, min_time_s_);
     max_time_s_ = sc::get<double>("max_time_s", params, max_time_s_);
@@ -74,32 +72,33 @@ void CPA::init(std::map<std::string, std::string> &params) {
 
 bool CPA::step_metrics(double t, double dt) {
     // First verify that within time constraints, if they are set
-    bool outside_time = (((min_time_s_ >= 0.0) && (t < min_time_s_)) || ((max_time_s_ > 0.0) && (t > max_time_s_)));
+    bool outside_time =
+        (((min_time_s_ >= 0.0) && (t < min_time_s_)) || ((max_time_s_ > 0.0) && (t > max_time_s_)));
     if (!outside_time) {
         if (!initialized_) {
-            for (auto &kv : *id_to_ent_map_) {
+            for (auto& kv : *id_to_ent_map_) {
                 cpa_map_[kv.first] = CPAData();
             }
             std::string log_dir = ((*id_to_ent_map_)[1])->mp()->log_dir();
             csv_.open_output(log_dir + "/" + "cpa.csv");
-            csv_.set_column_headers(scrimmage::CSV::Headers{
-                    "entity",
-                    "cpa",
-                    "closest_entity",
-                    "time"});
+            csv_.set_column_headers(
+                scrimmage::CSV::Headers{"entity", "cpa", "closest_entity", "time"});
             initialized_ = true;
         }
 
-        for (auto &kv : *id_to_ent_map_) {
-            // Checks if a specific team ID is not being checked or if the entity has the specific team ID
+        for (auto& kv : *id_to_ent_map_) {
+            // Checks if a specific team ID is not being checked or if the entity has the specific
+            // team ID
             if ((team_id_ <= 0) || (kv.second->id().team_id() == team_id_)) {
-                for (auto &kv2 : *id_to_ent_map_) {
+                for (auto& kv2 : *id_to_ent_map_) {
                     if (kv != kv2) {
                         // Checks if teams are not being checked or if the teams are the same
-                        if (!team_cpa_ || (kv.second->id().team_id() == kv2.second->id().team_id())) {
+                        if (!team_cpa_
+                            || (kv.second->id().team_id() == kv2.second->id().team_id())) {
                             if (kv.second->state_truth() && kv2.second->state_truth()) {
-                                double cur_distance = (kv.second->state_truth()->pos() -
-                                        kv2.second->state_truth()->pos()).norm();
+                                double cur_distance = (kv.second->state_truth()->pos()
+                                                       - kv2.second->state_truth()->pos())
+                                                          .norm();
                                 if (cur_distance < cpa_map_[kv.first].distance()) {
                                     cpa_map_[kv.first].set_distance(cur_distance);
                                     cpa_map_[kv.first].set_closest_entity(kv2.first);
@@ -116,17 +115,16 @@ bool CPA::step_metrics(double t, double dt) {
 }
 
 void CPA::calc_team_scores() {
-    for (auto &kv: cpa_map_) {
-        csv_.append(scrimmage::CSV::Pairs{
-                {"entity", kv.first},
-                {"cpa", kv.second.distance()},
-                {"closest_entity", kv.second.closest_entity()},
-                {"time", kv.second.time()}});
+    for (auto& kv : cpa_map_) {
+        csv_.append(scrimmage::CSV::Pairs{{"entity", kv.first},
+                                          {"cpa", kv.second.distance()},
+                                          {"closest_entity", kv.second.closest_entity()},
+                                          {"time", kv.second.time()}});
     }
 }
 
 void CPA::print_team_summaries() {
-    for (auto &kv: cpa_map_) {
+    for (auto& kv : cpa_map_) {
         cout << "Entity: " << kv.first;
         cout << " | CPA: " << kv.second.distance();
         cout << " | Closest Entity: " << kv.second.closest_entity();
@@ -134,5 +132,5 @@ void CPA::print_team_summaries() {
     }
     cout << sc::generate_chars("-", 70) << endl;
 }
-} // namespace metrics
-} // namespace scrimmage
+}  // namespace metrics
+}  // namespace scrimmage
diff --git a/src/plugins/metrics/FlagCaptureMetrics/FlagCaptureMetrics.cpp b/src/plugins/metrics/FlagCaptureMetrics/FlagCaptureMetrics.cpp
index 3a6eaacab6..1e8ca9a59c 100644
--- a/src/plugins/metrics/FlagCaptureMetrics/FlagCaptureMetrics.cpp
+++ b/src/plugins/metrics/FlagCaptureMetrics/FlagCaptureMetrics.cpp
@@ -30,20 +30,18 @@
  *
  */
 
-#include <scrimmage/plugins/metrics/FlagCaptureMetrics/FlagCaptureMetrics.h>
-
-#include <scrimmage/plugin_manager/RegisterPlugin.h>
+#include <scrimmage/common/Utilities.h>
 #include <scrimmage/entity/Entity.h>
 #include <scrimmage/math/State.h>
-#include <scrimmage/parse/ParseUtils.h>
-#include <scrimmage/common/Utilities.h>
 #include <scrimmage/metrics/Metrics.h>
 #include <scrimmage/msgs/Capture.pb.h>
-
-#include <scrimmage/pubsub/Message.h>
-#include <scrimmage/pubsub/Subscriber.h>
 #include <scrimmage/msgs/Collision.pb.h>
 #include <scrimmage/msgs/Event.pb.h>
+#include <scrimmage/parse/ParseUtils.h>
+#include <scrimmage/plugin_manager/RegisterPlugin.h>
+#include <scrimmage/plugins/metrics/FlagCaptureMetrics/FlagCaptureMetrics.h>
+#include <scrimmage/pubsub/Message.h>
+#include <scrimmage/pubsub/Subscriber.h>
 
 #include <iostream>
 #include <limits>
@@ -54,8 +52,7 @@ using std::endl;
 namespace sc = scrimmage;
 namespace sm = scrimmage_msgs;
 
-REGISTER_PLUGIN(scrimmage::Metrics,
-                scrimmage::metrics::FlagCaptureMetrics,
+REGISTER_PLUGIN(scrimmage::Metrics, scrimmage::metrics::FlagCaptureMetrics,
                 FlagCaptureMetrics_plugin)
 
 namespace scrimmage {
@@ -64,15 +61,15 @@ namespace metrics {
 FlagCaptureMetrics::FlagCaptureMetrics() {
 }
 
-void FlagCaptureMetrics::init(std::map<std::string, std::string> &params) {
+void FlagCaptureMetrics::init(std::map<std::string, std::string>& params) {
     params_ = params;
 
-    auto flag_taken_cb = [&] (scrimmage::MessagePtr<sm::FlagTaken> msg) {
+    auto flag_taken_cb = [&](scrimmage::MessagePtr<sm::FlagTaken> msg) {
         scores_[msg->data.entity_id()].increment_flags_taken();
     };
     subscribe<sm::FlagTaken>("GlobalNetwork", "FlagTaken", flag_taken_cb);
 
-    auto flag_captured_cb = [&] (scrimmage::MessagePtr<sm::FlagCaptured> msg) {
+    auto flag_captured_cb = [&](scrimmage::MessagePtr<sm::FlagCaptured> msg) {
         scores_[msg->data.entity_id()].increment_flags_captured();
     };
     subscribe<sm::FlagCaptured>("GlobalNetwork", "FlagCaptured", flag_captured_cb);
@@ -83,8 +80,8 @@ bool FlagCaptureMetrics::step_metrics(double t, double dt) {
 }
 
 void FlagCaptureMetrics::calc_team_scores() {
-    for (auto &kv : scores_) {
-        Score &score = kv.second;
+    for (auto& kv : scores_) {
+        Score& score = kv.second;
 
         int team_id = (*id_to_team_map_)[kv.first];
 
@@ -98,9 +95,9 @@ void FlagCaptureMetrics::calc_team_scores() {
         team_flag_scores_[team_id].add_flags_captured(score.flags_captured());
     }
 
-    for (auto &kv : team_flag_scores_) {
+    for (auto& kv : team_flag_scores_) {
         int team_id = kv.first;
-        Score &score = kv.second;
+        Score& score = kv.second;
         team_metrics_[team_id]["flags_taken"] = score.flags_taken();
         team_metrics_[team_id]["flags_captured"] = score.flags_captured();
         team_scores_[team_id] = score.score();
@@ -114,7 +111,6 @@ void FlagCaptureMetrics::calc_team_scores() {
 void FlagCaptureMetrics::print_team_summaries() {
     for (std::map<int, Score>::iterator it = team_flag_scores_.begin();
          it != team_flag_scores_.end(); ++it) {
-
         cout << "Team ID: " << it->first << endl;
         cout << "Score: " << it->second.score() << endl;
         cout << "Flags Taken: " << it->second.flags_taken() << endl;
@@ -122,5 +118,5 @@ void FlagCaptureMetrics::print_team_summaries() {
         cout << sc::generate_chars("-", 70) << endl;
     }
 }
-} // namespace metrics
-} // namespace scrimmage
+}  // namespace metrics
+}  // namespace scrimmage
diff --git a/src/plugins/metrics/OpenAIRewards/OpenAIRewards.cpp b/src/plugins/metrics/OpenAIRewards/OpenAIRewards.cpp
index aecce28818..0a257c790e 100644
--- a/src/plugins/metrics/OpenAIRewards/OpenAIRewards.cpp
+++ b/src/plugins/metrics/OpenAIRewards/OpenAIRewards.cpp
@@ -30,21 +30,19 @@
  *
  */
 
-#include <scrimmage/plugins/metrics/OpenAIRewards/OpenAIRewards.h>
-
 #include <scrimmage/common/CSV.h>
-#include <scrimmage/plugin_manager/RegisterPlugin.h>
+#include <scrimmage/common/Utilities.h>
 #include <scrimmage/entity/Entity.h>
 #include <scrimmage/math/State.h>
-#include <scrimmage/parse/ParseUtils.h>
-#include <scrimmage/parse/MissionParse.h>
-#include <scrimmage/common/Utilities.h>
 #include <scrimmage/metrics/Metrics.h>
-
-#include <scrimmage/pubsub/Message.h>
-#include <scrimmage/pubsub/Subscriber.h>
 #include <scrimmage/msgs/Collision.pb.h>
 #include <scrimmage/msgs/Event.pb.h>
+#include <scrimmage/parse/MissionParse.h>
+#include <scrimmage/parse/ParseUtils.h>
+#include <scrimmage/plugin_manager/RegisterPlugin.h>
+#include <scrimmage/plugins/metrics/OpenAIRewards/OpenAIRewards.h>
+#include <scrimmage/pubsub/Message.h>
+#include <scrimmage/pubsub/Subscriber.h>
 
 #include <iostream>
 #include <limits>
@@ -55,9 +53,7 @@ using std::endl;
 namespace sc = scrimmage;
 namespace sm = scrimmage_msgs;
 
-REGISTER_PLUGIN(scrimmage::Metrics,
-                scrimmage::metrics::OpenAIRewards,
-                OpenAIRewards_plugin)
+REGISTER_PLUGIN(scrimmage::Metrics, scrimmage::metrics::OpenAIRewards, OpenAIRewards_plugin)
 
 namespace scrimmage {
 namespace metrics {
@@ -66,7 +62,7 @@ OpenAIRewards::OpenAIRewards() : Metrics() {
     print_team_summary_ = false;
 }
 
-void OpenAIRewards::init(std::map<std::string, std::string> &/*params*/) {
+void OpenAIRewards::init(std::map<std::string, std::string>& /*params*/) {
     auto cb = [&](auto msg) {
         size_t id;
         double reward;
@@ -83,7 +79,7 @@ void OpenAIRewards::init(std::map<std::string, std::string> &/*params*/) {
 }
 
 void OpenAIRewards::print_team_summaries() {
-    for (auto &kv : rewards_) {
+    for (auto& kv : rewards_) {
         std::cout << "Reward for id " << kv.first << " = " << kv.second << std::endl;
     }
 }
@@ -93,14 +89,14 @@ void OpenAIRewards::calc_team_scores() {
     std::string filename = parent_->mp()->log_dir() + "/rewards.csv";
 
     if (!csv.open_output(filename)) {
-       std::cout << "Couldn't create output file" << endl;
+        std::cout << "Couldn't create output file" << endl;
     }
 
     csv.set_column_headers("id, reward");
-    for (auto &kv : rewards_) {
+    for (auto& kv : rewards_) {
         csv.append(CSV::Pairs{{"id", kv.first}, {"reward", kv.second}});
     }
     csv.close_output();
 }
-} // namespace metrics
-} // namespace scrimmage
+}  // namespace metrics
+}  // namespace scrimmage
diff --git a/src/plugins/metrics/SimpleCaptureMetrics/SimpleCaptureMetrics.cpp b/src/plugins/metrics/SimpleCaptureMetrics/SimpleCaptureMetrics.cpp
index 5c3051a0fc..0698b06067 100644
--- a/src/plugins/metrics/SimpleCaptureMetrics/SimpleCaptureMetrics.cpp
+++ b/src/plugins/metrics/SimpleCaptureMetrics/SimpleCaptureMetrics.cpp
@@ -30,19 +30,17 @@
  *
  */
 
-#include <scrimmage/plugin_manager/RegisterPlugin.h>
+#include <scrimmage/common/Utilities.h>
 #include <scrimmage/entity/Entity.h>
 #include <scrimmage/math/State.h>
-#include <scrimmage/parse/ParseUtils.h>
-#include <scrimmage/common/Utilities.h>
 #include <scrimmage/metrics/Metrics.h>
-
-#include <scrimmage/pubsub/Message.h>
-#include <scrimmage/pubsub/Subscriber.h>
 #include <scrimmage/msgs/Capture.pb.h>
 #include <scrimmage/msgs/Event.pb.h>
-
+#include <scrimmage/parse/ParseUtils.h>
+#include <scrimmage/plugin_manager/RegisterPlugin.h>
 #include <scrimmage/plugins/metrics/SimpleCaptureMetrics/SimpleCaptureMetrics.h>
+#include <scrimmage/pubsub/Message.h>
+#include <scrimmage/pubsub/Subscriber.h>
 
 #include <iostream>
 #include <limits>
@@ -53,31 +51,29 @@ using std::endl;
 namespace sc = scrimmage;
 namespace sm = scrimmage_msgs;
 
-REGISTER_PLUGIN(scrimmage::Metrics,
-        scrimmage::metrics::SimpleCaptureMetrics, SimpleCaptureMetrics_plugin)
+REGISTER_PLUGIN(scrimmage::Metrics, scrimmage::metrics::SimpleCaptureMetrics,
+                SimpleCaptureMetrics_plugin)
 
 namespace scrimmage {
 namespace metrics {
 
-void SimpleCaptureMetrics::init(std::map<std::string, std::string> &params) {
+void SimpleCaptureMetrics::init(std::map<std::string, std::string>& params) {
     params_ = params;
 
-    auto teamcapture_cb = [&] (scrimmage::MessagePtr<sm::TeamCapture> msg) {
+    auto teamcapture_cb = [&](scrimmage::MessagePtr<sm::TeamCapture> msg) {
         scores_[msg->data.source_id()].increment_count("TeamCapture");
     };
     subscribe<sm::TeamCapture>("GlobalNetwork", "TeamCapture", teamcapture_cb);
 
-    auto nonteamcapture_cb = [&] (
-            scrimmage::MessagePtr<sm::NonTeamCapture> msg) {
+    auto nonteamcapture_cb = [&](scrimmage::MessagePtr<sm::NonTeamCapture> msg) {
         scores_[msg->data.source_id()].increment_count("NonTeamCapture");
     };
-    subscribe<sm::NonTeamCapture>("GlobalNetwork", "NonTeamCapture",
-            nonteamcapture_cb);
+    subscribe<sm::NonTeamCapture>("GlobalNetwork", "NonTeamCapture", nonteamcapture_cb);
 }
 
 bool SimpleCaptureMetrics::step_metrics(double t, double dt) {
     if (!initialized_) {
-        for (auto & teamnum : *id_to_team_map_) {
+        for (auto& teamnum : *id_to_team_map_) {
             // get all the teams so we can initialize their scores
             teams_.emplace(teamnum.second);
         }
@@ -88,7 +84,7 @@ bool SimpleCaptureMetrics::step_metrics(double t, double dt) {
 
 void SimpleCaptureMetrics::calc_team_scores() {
     // Create the score if there isn't one yet
-    for (auto &team_id : teams_) {
+    for (auto& team_id : teams_) {
         if (team_scores_map_.count(team_id) == 0) {
             Score score;
             score.set_weights(params_);
@@ -96,23 +92,20 @@ void SimpleCaptureMetrics::calc_team_scores() {
         }
     }
 
-    for (auto &kv : scores_) {
-        Score &score = kv.second;
+    for (auto& kv : scores_) {
+        Score& score = kv.second;
 
         int team_id = (*id_to_team_map_)[kv.first];
 
-        team_scores_map_[team_id].add_count("TeamCapture",
-                score.count("TeamCapture"));
-        team_scores_map_[team_id].add_count("NonTeamCapture",
-                score.count("NonTeamCapture"));
+        team_scores_map_[team_id].add_count("TeamCapture", score.count("TeamCapture"));
+        team_scores_map_[team_id].add_count("NonTeamCapture", score.count("NonTeamCapture"));
     }
 
-    for (auto &kv : team_scores_map_) {
+    for (auto& kv : team_scores_map_) {
         int team_id = kv.first;
-        Score &score = kv.second;
+        Score& score = kv.second;
         team_metrics_[team_id]["TeamCapture"] = score.count("TeamCapture");
-        team_metrics_[team_id]["NonTeamCapture"] =
-            score.count("NonTeamCapture");
+        team_metrics_[team_id]["NonTeamCapture"] = score.count("NonTeamCapture");
 
         double s = score.score();
         team_scores_[team_id] = s;
@@ -126,7 +119,7 @@ void SimpleCaptureMetrics::calc_team_scores() {
 void SimpleCaptureMetrics::print_team_summaries() {
     for (auto kv : team_scores_map_) {
         int team_id = kv.first;
-        Score &score = kv.second;
+        Score& score = kv.second;
 
         cout << "Team ID: " << team_id << endl;
         cout << "Score: " << score.score() << endl;
diff --git a/src/plugins/metrics/SimpleCollisionMetrics/SimpleCollisionMetrics.cpp b/src/plugins/metrics/SimpleCollisionMetrics/SimpleCollisionMetrics.cpp
index ba98968852..12680fc55d 100644
--- a/src/plugins/metrics/SimpleCollisionMetrics/SimpleCollisionMetrics.cpp
+++ b/src/plugins/metrics/SimpleCollisionMetrics/SimpleCollisionMetrics.cpp
@@ -30,25 +30,22 @@
  *
  */
 
-#include <scrimmage/metrics/Metrics.h>
-
-#include <scrimmage/entity/Entity.h>
-#include <scrimmage/math/State.h>
 #include <scrimmage/common/RTree.h>
 #include <scrimmage/common/Time.h>
-#include <scrimmage/plugin_manager/RegisterPlugin.h>
 #include <scrimmage/common/Utilities.h>
-#include <scrimmage/parse/ParseUtils.h>
-
-#include <scrimmage/pubsub/Message.h>
-#include <scrimmage/pubsub/Subscriber.h>
+#include <scrimmage/entity/Entity.h>
+#include <scrimmage/math/State.h>
+#include <scrimmage/metrics/Metrics.h>
 #include <scrimmage/msgs/Collision.pb.h>
 #include <scrimmage/msgs/Event.pb.h>
-
+#include <scrimmage/parse/ParseUtils.h>
+#include <scrimmage/plugin_manager/RegisterPlugin.h>
 #include <scrimmage/plugins/metrics/SimpleCollisionMetrics/SimpleCollisionMetrics.h>
+#include <scrimmage/pubsub/Message.h>
+#include <scrimmage/pubsub/Subscriber.h>
 
-#include <iostream>
 #include <fstream>
+#include <iostream>
 
 namespace sc = scrimmage;
 namespace sm = scrimmage_msgs;
@@ -56,60 +53,54 @@ using std::cout;
 using std::endl;
 
 REGISTER_PLUGIN(scrimmage::Metrics, scrimmage::metrics::SimpleCollisionMetrics,
-        SimpleCollisionMetrics_plugin)
+                SimpleCollisionMetrics_plugin)
 
 namespace scrimmage {
 namespace metrics {
 
-SimpleCollisionMetrics::SimpleCollisionMetrics() {}
+SimpleCollisionMetrics::SimpleCollisionMetrics() {
+}
 
-void SimpleCollisionMetrics::init(std::map<std::string, std::string> &params) {
+void SimpleCollisionMetrics::init(std::map<std::string, std::string>& params) {
     params_ = params;
 
-    auto teamcoll_cb = [&] (scrimmage::MessagePtr<sm::TeamCollision> msg) {
+    auto teamcoll_cb = [&](scrimmage::MessagePtr<sm::TeamCollision> msg) {
         scores_[msg->data.entity_id_1()].increment_team_collisions();
         scores_[msg->data.entity_id_2()].increment_team_collisions();
     };
     subscribe<sm::TeamCollision>("GlobalNetwork", "TeamCollision", teamcoll_cb);
 
-    auto nonteamcoll_cb =
-        [&] (scrimmage::MessagePtr<sm::NonTeamCollision> msg) {
+    auto nonteamcoll_cb = [&](scrimmage::MessagePtr<sm::NonTeamCollision> msg) {
         scores_[msg->data.entity_id_1()].increment_non_team_collisions();
         scores_[msg->data.entity_id_2()].increment_non_team_collisions();
     };
-    subscribe<sm::NonTeamCollision>("GlobalNetwork", "NonTeamCollision",
-            nonteamcoll_cb);
+    subscribe<sm::NonTeamCollision>("GlobalNetwork", "NonTeamCollision", nonteamcoll_cb);
 
-    auto groundcoll_cb = [&] (scrimmage::MessagePtr<sm::GroundCollision> msg) {
+    auto groundcoll_cb = [&](scrimmage::MessagePtr<sm::GroundCollision> msg) {
         scores_[msg->data.entity_id()].increment_ground_collisions();
     };
-    subscribe<sm::GroundCollision>("GlobalNetwork", "GroundCollision",
-            groundcoll_cb);
+    subscribe<sm::GroundCollision>("GlobalNetwork", "GroundCollision", groundcoll_cb);
 
-    auto entitygen_cb = [&] (scrimmage::MessagePtr<sm::EntityGenerated> msg) {
+    auto entitygen_cb = [&](scrimmage::MessagePtr<sm::EntityGenerated> msg) {
         scores_[msg->data.entity_id()].set_flight_time_start(msg->time);
     };
-    subscribe<sm::EntityGenerated>("GlobalNetwork", "EntityGenerated",
-            entitygen_cb);
+    subscribe<sm::EntityGenerated>("GlobalNetwork", "EntityGenerated", entitygen_cb);
 
-    auto entityrem_cb = [&] (scrimmage::MessagePtr<sm::EntityRemoved> msg) {
+    auto entityrem_cb = [&](scrimmage::MessagePtr<sm::EntityRemoved> msg) {
         scores_[msg->data.entity_id()].set_flight_time_end(msg->time);
     };
-    subscribe<sm::EntityRemoved>("GlobalNetwork", "EntityRemoved",
-            entityrem_cb);
+    subscribe<sm::EntityRemoved>("GlobalNetwork", "EntityRemoved", entityrem_cb);
 
-    auto entity_pre_end_cb =
-        [&] (scrimmage::MessagePtr<sm::EntityPresentAtEnd> msg) {
+    auto entity_pre_end_cb = [&](scrimmage::MessagePtr<sm::EntityPresentAtEnd> msg) {
         scores_[msg->data.entity_id()].set_flight_time_end(time_->t());
         surviving_teams_[(*id_to_team_map_)[msg->data.entity_id()]] = true;
     };
-    subscribe<sm::EntityPresentAtEnd>("GlobalNetwork", "EntityPresentAtEnd",
-            entity_pre_end_cb);
+    subscribe<sm::EntityPresentAtEnd>("GlobalNetwork", "EntityPresentAtEnd", entity_pre_end_cb);
 }
 
 bool SimpleCollisionMetrics::step_metrics(double t, double dt) {
     if (!initialized_) {
-        for (auto & teamnum : *id_to_team_map_) {
+        for (auto& teamnum : *id_to_team_map_) {
             // get all the teams so we can initialize their scores
             teams_.emplace(teamnum.second);
         }
@@ -121,7 +112,7 @@ bool SimpleCollisionMetrics::step_metrics(double t, double dt) {
 void SimpleCollisionMetrics::calc_team_scores() {
     double end_time = -std::numeric_limits<double>::infinity();
     double beg_time = std::numeric_limits<double>::infinity();
-    for (auto &kv : scores_) {
+    for (auto& kv : scores_) {
         double temp_beg_time = kv.second.flight_time_start();
         double temp_end_time = kv.second.flight_time_end();
         if (temp_beg_time < beg_time) {
@@ -141,7 +132,7 @@ void SimpleCollisionMetrics::calc_team_scores() {
     double max_flight_time = end_time - beg_time;
 
     // Create the score, if necessary
-    for (auto &team_id : teams_) {
+    for (auto& team_id : teams_) {
         if (team_coll_scores_.count(team_id) == 0) {
             SimpleCollisionScore score;
             score.set_weights(params_);
@@ -151,8 +142,8 @@ void SimpleCollisionMetrics::calc_team_scores() {
         }
     }
 
-    for (auto &kv : scores_) {
-        SimpleCollisionScore &score = kv.second;
+    for (auto& kv : scores_) {
+        SimpleCollisionScore& score = kv.second;
 
         // If the end time is less than start time, the flight time was never
         // set. Use the ending simulation time.
@@ -162,19 +153,16 @@ void SimpleCollisionMetrics::calc_team_scores() {
 
         int team_id = (*id_to_team_map_)[kv.first];
 
-        team_coll_scores_[team_id].add_non_team_collisions(
-                score.non_team_collisions());
-        team_coll_scores_[team_id].add_team_collisions(
-                score.team_collisions());
-        team_coll_scores_[team_id].add_ground_collisions(
-                score.ground_collisions());
+        team_coll_scores_[team_id].add_non_team_collisions(score.non_team_collisions());
+        team_coll_scores_[team_id].add_team_collisions(score.team_collisions());
+        team_coll_scores_[team_id].add_ground_collisions(score.ground_collisions());
         team_coll_scores_[team_id].add_flight_time(score.flight_time());
         team_coll_scores_[team_id].increment_entity_count();
     }
 
-    for (auto &kv : team_coll_scores_) {
+    for (auto& kv : team_coll_scores_) {
         int team_id = kv.first;
-        SimpleCollisionScore &score = kv.second;
+        SimpleCollisionScore& score = kv.second;
         team_metrics_[team_id]["entity_count"] = score.entity_count();
         team_metrics_[team_id]["flight_time"] = score.flight_time();
         team_metrics_[team_id]["flight_time_norm"] = score.flight_time_norm();
@@ -195,8 +183,8 @@ void SimpleCollisionMetrics::calc_team_scores() {
 }
 
 void SimpleCollisionMetrics::print_team_summaries() {
-    for (std::map<int, SimpleCollisionScore>::iterator it =
-            team_coll_scores_.begin(); it != team_coll_scores_.end(); ++it) {
+    for (std::map<int, SimpleCollisionScore>::iterator it = team_coll_scores_.begin();
+         it != team_coll_scores_.end(); ++it) {
         bool survived = false;
         auto it_survive = surviving_teams_.find(it->first);
         if (it_survive != surviving_teams_.end()) survived = true;
@@ -210,10 +198,8 @@ void SimpleCollisionMetrics::print_team_summaries() {
         cout << "Score: " << it->second.score() << endl;
         cout << "Entity Count: " << it->second.entity_count() << endl;
         cout << "Total Flight Time: " << it->second.flight_time() << endl;
-        cout << "Total Normalized Flight Time: " <<
-            it->second.flight_time_norm() << endl;
-        cout << "Non-Team Collisions: " << it->second.non_team_collisions() <<
-            endl;
+        cout << "Total Normalized Flight Time: " << it->second.flight_time_norm() << endl;
+        cout << "Non-Team Collisions: " << it->second.non_team_collisions() << endl;
         cout << "Team Collisions: " << it->second.team_collisions() << endl;
         cout << "Ground Collisions: " << it->second.ground_collisions() << endl;
         cout << sc::generate_chars("-", 70) << endl;
diff --git a/src/plugins/metrics/SimpleCollisionMetrics/SimpleCollisionScore.cpp b/src/plugins/metrics/SimpleCollisionMetrics/SimpleCollisionScore.cpp
index 309b337257..7012e609cd 100644
--- a/src/plugins/metrics/SimpleCollisionMetrics/SimpleCollisionScore.cpp
+++ b/src/plugins/metrics/SimpleCollisionMetrics/SimpleCollisionScore.cpp
@@ -39,7 +39,7 @@ namespace sc = scrimmage;
 namespace scrimmage {
 namespace metrics {
 
-bool SimpleCollisionScore::set_weights(std::map<std::string, std::string> &params) {
+bool SimpleCollisionScore::set_weights(std::map<std::string, std::string>& params) {
     flight_time_w_ = sc::get<double>("flight_time_w", params, 0.0);
     team_collisions_w_ = sc::get<double>("team_collisions_w", params, 0.0);
     non_team_collisions_w_ = sc::get<double>("non_team_collisions_w", params, 0.0);
@@ -48,10 +48,9 @@ bool SimpleCollisionScore::set_weights(std::map<std::string, std::string> &param
 }
 
 double SimpleCollisionScore::score() {
-    double s = flight_time_norm() * flight_time_w_
-        + non_team_collisions() * non_team_collisions_w_
-        + team_collisions() * team_collisions_w_;
+    double s = flight_time_norm() * flight_time_w_ + non_team_collisions() * non_team_collisions_w_
+               + team_collisions() * team_collisions_w_;
     return s;
 }
-} // namespace metrics
-} // namespace scrimmage
+}  // namespace metrics
+}  // namespace scrimmage
diff --git a/src/plugins/motion/Ballistic/Ballistic.cpp b/src/plugins/motion/Ballistic/Ballistic.cpp
index 64a8390c1d..f96bb0c0d6 100644
--- a/src/plugins/motion/Ballistic/Ballistic.cpp
+++ b/src/plugins/motion/Ballistic/Ballistic.cpp
@@ -30,42 +30,29 @@
  *
  */
 
-#include <scrimmage/plugins/motion/Ballistic/Ballistic.h>
 #include <scrimmage/common/Utilities.h>
-#include <scrimmage/parse/ParseUtils.h>
-#include <scrimmage/math/Angles.h>
-#include <scrimmage/plugin_manager/RegisterPlugin.h>
 #include <scrimmage/entity/Entity.h>
+#include <scrimmage/math/Angles.h>
 #include <scrimmage/math/State.h>
+#include <scrimmage/parse/ParseUtils.h>
+#include <scrimmage/plugin_manager/RegisterPlugin.h>
+#include <scrimmage/plugins/motion/Ballistic/Ballistic.h>
 
 #include <boost/algorithm/clamp.hpp>
 
 namespace sc = scrimmage;
 
-REGISTER_PLUGIN(scrimmage::MotionModel,
-                scrimmage::motion::Ballistic,
-                Ballistic_plugin)
+REGISTER_PLUGIN(scrimmage::MotionModel, scrimmage::motion::Ballistic, Ballistic_plugin)
 
 using boost::algorithm::clamp;
 
 namespace scrimmage {
 namespace motion {
 
-enum ModelParams {
-    X = 0,
-    Y,
-    Z,
-    X_vel,
-    Y_vel,
-    Z_vel,
-    X_acc,
-    Y_acc,
-    Z_acc,
-    MODEL_NUM_ITEMS
-};
-
-bool Ballistic::init(std::map<std::string, std::string> &info,
-                     std::map<std::string, std::string> &params) {
+enum ModelParams { X = 0, Y, Z, X_vel, Y_vel, Z_vel, X_acc, Y_acc, Z_acc, MODEL_NUM_ITEMS };
+
+bool Ballistic::init(std::map<std::string, std::string>& info,
+                     std::map<std::string, std::string>& params) {
     x_.resize(MODEL_NUM_ITEMS);
     x_[X] = state_->pos()(0);
     x_[Y] = state_->pos()(1);
@@ -105,8 +92,7 @@ bool Ballistic::step(double time, double dt) {
     return true;
 }
 
-void Ballistic::model(const vector_t &x , vector_t &dxdt ,
-                                double t) {
+void Ballistic::model(const vector_t& x, vector_t& dxdt, double t) {
     dxdt[X] = x[X_vel];
     dxdt[Y] = x[Y_vel];
     dxdt[Z] = x[Z_vel];
@@ -119,5 +105,5 @@ void Ballistic::model(const vector_t &x , vector_t &dxdt ,
     dxdt[Y_acc] = 0;
     dxdt[Z_acc] = -mass_ * g_;
 }
-} // namespace motion
-} // namespace scrimmage
+}  // namespace motion
+}  // namespace scrimmage
diff --git a/src/plugins/motion/DoubleIntegrator/DoubleIntegrator.cpp b/src/plugins/motion/DoubleIntegrator/DoubleIntegrator.cpp
index 3a127081d4..dc32f48fe1 100644
--- a/src/plugins/motion/DoubleIntegrator/DoubleIntegrator.cpp
+++ b/src/plugins/motion/DoubleIntegrator/DoubleIntegrator.cpp
@@ -31,15 +31,17 @@
  */
 
 #include <scrimmage/common/Utilities.h>
-#include <scrimmage/parse/ParseUtils.h>
-#include <scrimmage/plugin_manager/RegisterPlugin.h>
 #include <scrimmage/entity/Entity.h>
-#include <scrimmage/math/State.h>
 #include <scrimmage/math/Angles.h>
+#include <scrimmage/math/State.h>
+#include <scrimmage/parse/ParseUtils.h>
+#include <scrimmage/plugin_manager/RegisterPlugin.h>
 #include <scrimmage/plugins/motion/DoubleIntegrator/DoubleIntegrator.h>
+
 #include <boost/algorithm/clamp.hpp>
 
-REGISTER_PLUGIN(scrimmage::MotionModel, scrimmage::motion::DoubleIntegrator, DoubleIntegrator_plugin)
+REGISTER_PLUGIN(scrimmage::MotionModel, scrimmage::motion::DoubleIntegrator,
+                DoubleIntegrator_plugin)
 
 namespace scrimmage {
 namespace motion {
@@ -48,20 +50,21 @@ namespace pl = std::placeholders;
 using boost::algorithm::clamp;
 namespace sc = scrimmage;
 
-enum ModelParams {X, Y, Z, VX, VY, VZ, YAW, YAW_DOT, STATE_SIZE};
+enum ModelParams { X, Y, Z, VX, VY, VZ, YAW, YAW_DOT, STATE_SIZE };
 
 DoubleIntegrator::DoubleIntegrator() : motion_model_sets_yaw_(false) {
     x_.resize(STATE_SIZE);
 }
 
-bool DoubleIntegrator::init(std::map<std::string, std::string> &info,
-                            std::map<std::string, std::string> &params) {
+bool DoubleIntegrator::init(std::map<std::string, std::string>& info,
+                            std::map<std::string, std::string>& params) {
     max_vel_ = std::stod(params.at("max_vel"));
     max_acc_ = std::stod(params.at("max_acc"));
     motion_model_sets_yaw_ = sc::str2bool(params.at("motion_model_sets_yaw"));
     sim_copter_orientation_ = sc::get<bool>("sim_copter_orientation", params, false);
     sim_copter_max_roll_ = sc::Angles::deg2rad(sc::get<double>("sim_copter_max_roll", params, 30));
-    sim_copter_max_pitch_ = sc::Angles::deg2rad(sc::get<double>("sim_copter_max_pitch", params, 30));
+    sim_copter_max_pitch_ =
+        sc::Angles::deg2rad(sc::get<double>("sim_copter_max_pitch", params, 30));
     max_yaw_vel_ = sc::get<double>("max_yaw_vel", params, 1.0);
     max_yaw_acc_ = sc::get<double>("max_yaw_acc", params, 1.0);
 
@@ -138,7 +141,7 @@ double DoubleIntegrator::update_dvdt(double vel, double max_vel, double acc) {
     }
 }
 
-void DoubleIntegrator::model(const vector_t &x , vector_t &dxdt , double t) {
+void DoubleIntegrator::model(const vector_t& x, vector_t& dxdt, double t) {
     dxdt[X] = clamp(x_[VX], -max_vel_, max_vel_);
     dxdt[Y] = clamp(x_[VY], -max_vel_, max_vel_);
     dxdt[Z] = clamp(x_[VZ], -max_vel_, max_vel_);
@@ -150,7 +153,7 @@ void DoubleIntegrator::model(const vector_t &x , vector_t &dxdt , double t) {
     dxdt[YAW_DOT] = 0;
 }
 
-void DoubleIntegrator::teleport(sc::StatePtr &state) {
+void DoubleIntegrator::teleport(sc::StatePtr& state) {
     x_[X] = state->pos()[0];
     x_[Y] = state->pos()[1];
     x_[Z] = state->pos()[2];
@@ -160,5 +163,5 @@ void DoubleIntegrator::teleport(sc::StatePtr &state) {
     x_[YAW] = state->quat().yaw();
     x_[YAW_DOT] = 0;
 }
-} // namespace motion
-} // namespace scrimmage
+}  // namespace motion
+}  // namespace scrimmage
diff --git a/src/plugins/motion/DubinsAirplane/DubinsAirplane.cpp b/src/plugins/motion/DubinsAirplane/DubinsAirplane.cpp
index 4b1e553a5f..ee8b81a625 100644
--- a/src/plugins/motion/DubinsAirplane/DubinsAirplane.cpp
+++ b/src/plugins/motion/DubinsAirplane/DubinsAirplane.cpp
@@ -30,14 +30,15 @@
  *
  */
 
-#include <scrimmage/plugins/motion/DubinsAirplane/DubinsAirplane.h>
 #include <scrimmage/common/Utilities.h>
+#include <scrimmage/entity/Entity.h>
 #include <scrimmage/math/Angles.h>
-#include <scrimmage/parse/ParseUtils.h>
+#include <scrimmage/math/State.h>
 #include <scrimmage/parse/MissionParse.h>
+#include <scrimmage/parse/ParseUtils.h>
 #include <scrimmage/plugin_manager/RegisterPlugin.h>
-#include <scrimmage/entity/Entity.h>
-#include <scrimmage/math/State.h>
+#include <scrimmage/plugins/motion/DubinsAirplane/DubinsAirplane.h>
+
 #include <boost/algorithm/clamp.hpp>
 
 REGISTER_PLUGIN(scrimmage::MotionModel, scrimmage::motion::DubinsAirplane, DubinsAirplane_plugin)
@@ -49,19 +50,10 @@ namespace sc = scrimmage;
 
 namespace pl = std::placeholders;
 
-enum ModelParams {
-    X = 0,
-    Y,
-    Z,
-    ROLL,
-    PITCH,
-    YAW,
-    MODEL_NUM_ITEMS
-};
-
-bool DubinsAirplane::init(std::map<std::string, std::string> &info,
-                      std::map<std::string, std::string> &params) {
+enum ModelParams { X = 0, Y, Z, ROLL, PITCH, YAW, MODEL_NUM_ITEMS };
 
+bool DubinsAirplane::init(std::map<std::string, std::string>& info,
+                          std::map<std::string, std::string>& params) {
     // Model limits
     speed_max_ = sc::get<double>("speed_max", params, speed_max_);
     speed_min_ = sc::get<double>("speed_min", params, speed_min_);
@@ -88,14 +80,11 @@ bool DubinsAirplane::init(std::map<std::string, std::string> &info,
     x_[YAW] = state_->quat().yaw();
 
     if (write_csv_) {
-        csv_.open_output(parent_->mp()->log_dir() + "/"
-                         + std::to_string(parent_->id().id())
+        csv_.open_output(parent_->mp()->log_dir() + "/" + std::to_string(parent_->id().id())
                          + "-dubinsairplane-states.csv");
 
-        csv_.set_column_headers(sc::CSV::Headers{"t",
-                        "x", "y", "z",
-                        "roll", "pitch", "yaw",
-                        "speed"});
+        csv_.set_column_headers(
+            sc::CSV::Headers{"t", "x", "y", "z", "roll", "pitch", "yaw", "speed"});
     }
     return true;
 }
@@ -136,20 +125,19 @@ bool DubinsAirplane::step(double t, double dt) {
 
     if (write_csv_) {
         // Log state to CSV
-        csv_.append(sc::CSV::Pairs{
-                {"t", t},
-                {"x", state_->pos()(0)},
-                {"y", state_->pos()(1)},
-                {"z", state_->pos()(2)},
-                {"roll", state_->quat().roll()},
-                {"pitch", state_->quat().pitch()},
-                {"yaw", state_->quat().yaw()},
-                {"speed", speed_}});
+        csv_.append(sc::CSV::Pairs{{"t", t},
+                                   {"x", state_->pos()(0)},
+                                   {"y", state_->pos()(1)},
+                                   {"z", state_->pos()(2)},
+                                   {"roll", state_->quat().roll()},
+                                   {"pitch", state_->quat().pitch()},
+                                   {"yaw", state_->quat().yaw()},
+                                   {"speed", speed_}});
     }
     return true;
 }
 
-void DubinsAirplane::model(const vector_t &x , vector_t &dxdt , double t) {
+void DubinsAirplane::model(const vector_t& x, vector_t& dxdt, double t) {
     // The DubinsAirplane equations of motion are from the following paper:
     // Owen, Mark, Randal W. Beard, and Timothy W. McLain. "Implementing dubins
     // airplane paths on fixed-wing uavs." Handbook of unmanned aerial
@@ -169,5 +157,5 @@ void DubinsAirplane::model(const vector_t &x , vector_t &dxdt , double t) {
         dxdt[YAW] = -g_ / speed_ * tan(roll_);
     }
 }
-} // namespace motion
-} // namespace scrimmage
+}  // namespace motion
+}  // namespace scrimmage
diff --git a/src/plugins/motion/DubinsAirplane3D/DubinsAirplane3D.cpp b/src/plugins/motion/DubinsAirplane3D/DubinsAirplane3D.cpp
index 8ae2566648..953b23cbd6 100644
--- a/src/plugins/motion/DubinsAirplane3D/DubinsAirplane3D.cpp
+++ b/src/plugins/motion/DubinsAirplane3D/DubinsAirplane3D.cpp
@@ -30,16 +30,18 @@
  *
  */
 
-#include <scrimmage/plugins/motion/DubinsAirplane3D/DubinsAirplane3D.h>
 #include <scrimmage/common/Utilities.h>
-#include <scrimmage/parse/ParseUtils.h>
-#include <scrimmage/parse/MissionParse.h>
-#include <scrimmage/plugin_manager/RegisterPlugin.h>
 #include <scrimmage/entity/Entity.h>
 #include <scrimmage/math/State.h>
+#include <scrimmage/parse/MissionParse.h>
+#include <scrimmage/parse/ParseUtils.h>
+#include <scrimmage/plugin_manager/RegisterPlugin.h>
+#include <scrimmage/plugins/motion/DubinsAirplane3D/DubinsAirplane3D.h>
+
 #include <boost/algorithm/clamp.hpp>
 
-REGISTER_PLUGIN(scrimmage::MotionModel, scrimmage::motion::DubinsAirplane3D, DubinsAirplane3D_plugin)
+REGISTER_PLUGIN(scrimmage::MotionModel, scrimmage::motion::DubinsAirplane3D,
+                DubinsAirplane3D_plugin)
 
 namespace scrimmage {
 namespace motion {
@@ -52,9 +54,9 @@ enum ModelParams {
     U = 0,
     V,
     W,
-    P, // roll rate
-    Q, // pitch rate
-    R, // yaw rate
+    P,  // roll rate
+    Q,  // pitch rate
+    R,  // yaw rate
     Uw,
     Vw,
     Ww,
@@ -68,8 +70,8 @@ enum ModelParams {
     MODEL_NUM_ITEMS
 };
 
-bool DubinsAirplane3D::init(std::map<std::string, std::string> &info,
-                            std::map<std::string, std::string> &params) {
+bool DubinsAirplane3D::init(std::map<std::string, std::string>& info,
+                            std::map<std::string, std::string>& params) {
     write_csv_ = sc::get<bool>("write_csv", params, false);
 
     // Model limits
@@ -82,7 +84,7 @@ bool DubinsAirplane3D::init(std::map<std::string, std::string> &info,
     desired_roll_idx_ = vars_.declare(VariableIO::Type::desired_roll, VariableIO::Direction::In);
 
     x_.resize(MODEL_NUM_ITEMS);
-    Eigen::Vector3d &pos = state_->pos();
+    Eigen::Vector3d& pos = state_->pos();
     quat_world_ = state_->quat();
     quat_world_.normalize();
 
@@ -117,17 +119,12 @@ bool DubinsAirplane3D::init(std::map<std::string, std::string> &info,
     x_[q3] = quat_local_.z();
 
     if (write_csv_) {
-        csv_.open_output(parent_->mp()->log_dir() + "/"
-                         + std::to_string(parent_->id().id())
+        csv_.open_output(parent_->mp()->log_dir() + "/" + std::to_string(parent_->id().id())
                          + "-dubins-airplane3d-states.csv");
 
-        csv_.set_column_headers(sc::CSV::Headers{"t",
-                        "x", "y", "z",
-                        "U", "V", "W",
-                        "P", "Q", "R",
-                        "roll", "pitch", "yaw",
-                        "speed",
-                        "Uw", "Vw", "Ww"});
+        csv_.set_column_headers(sc::CSV::Headers{"t", "x", "y", "z", "U", "V", "W", "P", "Q", "R",
+                                                 "roll", "pitch", "yaw", "speed", "Uw", "Vw",
+                                                 "Ww"});
     }
     return true;
 }
@@ -199,30 +196,29 @@ bool DubinsAirplane3D::step(double t, double dt) {
 
     if (write_csv_) {
         // Log state to CSV
-        csv_.append(sc::CSV::Pairs{
-                {"t", t},
-                {"x", x_[Xw]},
-                {"y", x_[Yw]},
-                {"z", x_[Zw]},
-                {"U", x_[U]},
-                {"V", x_[V]},
-                {"W", x_[W]},
-                {"P", x_[P]},
-                {"Q", x_[Q]},
-                {"R", x_[R]},
-                {"roll", state_->quat().roll()},
-                {"pitch", state_->quat().pitch()},
-                {"yaw", state_->quat().yaw()},
-                {"speed", speed_},
-                {"Uw", state_->vel()(0)},
-                {"Vw", state_->vel()(1)},
-                {"Ww", state_->vel()(2)}});
+        csv_.append(sc::CSV::Pairs{{"t", t},
+                                   {"x", x_[Xw]},
+                                   {"y", x_[Yw]},
+                                   {"z", x_[Zw]},
+                                   {"U", x_[U]},
+                                   {"V", x_[V]},
+                                   {"W", x_[W]},
+                                   {"P", x_[P]},
+                                   {"Q", x_[Q]},
+                                   {"R", x_[R]},
+                                   {"roll", state_->quat().roll()},
+                                   {"pitch", state_->quat().pitch()},
+                                   {"yaw", state_->quat().yaw()},
+                                   {"speed", speed_},
+                                   {"Uw", state_->vel()(0)},
+                                   {"Vw", state_->vel()(1)},
+                                   {"Ww", state_->vel()(2)}});
     }
 
     return true;
 }
 
-void DubinsAirplane3D::model(const vector_t &x , vector_t &dxdt , double t) {
+void DubinsAirplane3D::model(const vector_t& x, vector_t& dxdt, double t) {
     dxdt[U] = 0;
     dxdt[V] = 0;
     dxdt[W] = 0;
@@ -232,10 +228,10 @@ void DubinsAirplane3D::model(const vector_t &x , vector_t &dxdt , double t) {
     dxdt[R] = 0;
 
     double lambda = 1 - (pow(x[q0], 2) + pow(x[q1], 2) + pow(x[q2], 2) + pow(x[q3], 2));
-    dxdt[q0] = -0.5 * (x[q1]*x[P] + x[q2]*x[Q] + x[q3]*x[R]) + lambda * x[q0];
-    dxdt[q1] = +0.5 * (x[q0]*x[P] + x[q2]*x[R] - x[q3]*x[Q]) + lambda * x[q1];
-    dxdt[q2] = +0.5 * (x[q0]*x[Q] + x[q3]*x[P] - x[q1]*x[R]) + lambda * x[q2];
-    dxdt[q3] = +0.5 * (x[q0]*x[R] + x[q1]*x[Q] - x[q2]*x[P]) + lambda * x[q3];
+    dxdt[q0] = -0.5 * (x[q1] * x[P] + x[q2] * x[Q] + x[q3] * x[R]) + lambda * x[q0];
+    dxdt[q1] = +0.5 * (x[q0] * x[P] + x[q2] * x[R] - x[q3] * x[Q]) + lambda * x[q1];
+    dxdt[q2] = +0.5 * (x[q0] * x[Q] + x[q3] * x[P] - x[q1] * x[R]) + lambda * x[q2];
+    dxdt[q3] = +0.5 * (x[q0] * x[R] + x[q1] * x[Q] - x[q2] * x[P]) + lambda * x[q3];
 
     // Local position / velocity to global
     // Normalize quaternion
@@ -254,5 +250,5 @@ void DubinsAirplane3D::model(const vector_t &x , vector_t &dxdt , double t) {
     dxdt[Yw] = vel_world(1);
     dxdt[Zw] = vel_world(2);
 }
-} // namespace motion
-} // namespace scrimmage
+}  // namespace motion
+}  // namespace scrimmage
diff --git a/src/plugins/motion/FixedWing6DOF/FixedWing6DOF.cpp b/src/plugins/motion/FixedWing6DOF/FixedWing6DOF.cpp
index 9a6b4eeaf9..7c001aea03 100644
--- a/src/plugins/motion/FixedWing6DOF/FixedWing6DOF.cpp
+++ b/src/plugins/motion/FixedWing6DOF/FixedWing6DOF.cpp
@@ -30,23 +30,21 @@
  *
  */
 
-#include <scrimmage/plugins/motion/FixedWing6DOF/FixedWing6DOF.h>
-#include <scrimmage/common/Utilities.h>
 #include <scrimmage/common/Time.h>
-#include <scrimmage/parse/ParseUtils.h>
-#include <scrimmage/plugin_manager/RegisterPlugin.h>
-#include <scrimmage/math/Angles.h>
+#include <scrimmage/common/Utilities.h>
 #include <scrimmage/entity/Entity.h>
+#include <scrimmage/math/Angles.h>
 #include <scrimmage/parse/MissionParse.h>
-#include <scrimmage/proto/Shape.pb.h>
+#include <scrimmage/parse/ParseUtils.h>
+#include <scrimmage/plugin_manager/RegisterPlugin.h>
+#include <scrimmage/plugins/motion/FixedWing6DOF/FixedWing6DOF.h>
 #include <scrimmage/proto/ProtoConversions.h>
+#include <scrimmage/proto/Shape.pb.h>
 
 #include <Eigen/Dense>
-
-#include <iostream>
-#include <iomanip>
-
 #include <boost/algorithm/clamp.hpp>
+#include <iomanip>
+#include <iostream>
 
 using std::cout;
 using std::endl;
@@ -70,8 +68,8 @@ std::tuple<int, int, int> FixedWing6DOF::version() {
     return std::tuple<int, int, int>(0, 0, 1);
 }
 
-bool FixedWing6DOF::init(std::map<std::string, std::string> &info,
-                          std::map<std::string, std::string> &params) {
+bool FixedWing6DOF::init(std::map<std::string, std::string>& info,
+                         std::map<std::string, std::string>& params) {
     // Setup variable index for controllers
     throttle_idx_ = vars_.declare(VariableIO::Type::throttle, VariableIO::Direction::In);
     elevator_idx_ = vars_.declare(VariableIO::Type::elevator, VariableIO::Direction::In);
@@ -93,7 +91,8 @@ bool FixedWing6DOF::init(std::map<std::string, std::string> &info,
     x_[V] = vel_body(1);
     x_[W] = vel_body(2);
 
-    Eigen::Vector3d local_ang_vel(state_->ang_vel()(0), -state_->ang_vel()(1), -state_->ang_vel()(2));
+    Eigen::Vector3d local_ang_vel(state_->ang_vel()(0), -state_->ang_vel()(1),
+                                  -state_->ang_vel()(2));
     local_ang_vel = quat_body_.rotate_reverse(local_ang_vel);
 
     x_[P] = local_ang_vel(0);
@@ -123,10 +122,9 @@ bool FixedWing6DOF::init(std::map<std::string, std::string> &info,
     // catapault parameters
     launch_accel_ = sc::get<double>("launch_accel", params, launch_accel_);
     launch_speed_ = sc::get<double>("launch_speed", params, launch_speed_);
-    launch_time_  = sc::get<double>("launch_time",  params, launch_time_);
+    launch_time_ = sc::get<double>("launch_time", params, launch_time_);
     bool use_launcher = sc::get<bool>("use_launcher", params, false);
-    if (true == use_launcher)
-        launch_state_ = PRELAUNCH;
+    if (true == use_launcher) launch_state_ = PRELAUNCH;
 
     use_ground_model_ = sc::get<double>("use_ground_model", params, true);
 
@@ -136,15 +134,13 @@ bool FixedWing6DOF::init(std::map<std::string, std::string> &info,
 
     // Get the inertia matrix
     std::vector<std::vector<std::string>> vecs;
-    std::string inertia_matrix = sc::get<std::string>("inertia_matrix",
-                                                      params, "");
+    std::string inertia_matrix = sc::get<std::string>("inertia_matrix", params, "");
 
     // If the inertia matrix is described using slug_ft_sq, it has priority
     bool convert_slug = false;
     if (params.count("inertia_matrix_slug_ft_sq")) {
         convert_slug = true;
-        inertia_matrix = sc::get<std::string>("inertia_matrix_slug_ft_sq",
-                                              params, "");
+        inertia_matrix = sc::get<std::string>("inertia_matrix_slug_ft_sq", params, "");
     }
 
     // Parse inertia matrix
@@ -162,7 +158,7 @@ bool FixedWing6DOF::init(std::map<std::string, std::string> &info,
                 I_(row, i) = std::stod(vec[i]);
                 // If the matrix was defined in slug*ft^2, convert it to SI
                 if (convert_slug) {
-                    I_(row, i) *= 1.35581795; // slug*ft^2 to kg*m^2
+                    I_(row, i) *= 1.35581795;  // slug*ft^2 to kg*m^2
                 }
             }
             row++;
@@ -184,27 +180,23 @@ bool FixedWing6DOF::init(std::map<std::string, std::string> &info,
     // Should we write a CSV file? What values should be written?
     write_csv_ = sc::get<bool>("write_csv", params, false);
     if (write_csv_) {
-        csv_.open_output(parent_->mp()->log_dir() + "/"
-                         + std::to_string(parent_->id().id())
+        csv_.open_output(parent_->mp()->log_dir() + "/" + std::to_string(parent_->id().id())
                          + "-states.csv");
         cout << "Writing log to " + parent_->mp()->log_dir() + "/"
-                         + std::to_string(parent_->id().id())
-                         + "-states.csv" << endl;
-
-        csv_.set_column_headers(sc::CSV::Headers{"t",
-                    "x", "y", "z",
-                    "U", "V", "W", "alpha", "alpha_dot", "beta",
-                    "P", "Q", "R",
-                    "Ax_b", "Ay_b", "Az_b",
-                    "AngAccelx_b", "AngAccely_b", "AngAccelz_b",
-                    "roll", "pitch", "yaw",
-                    "throttle", "thrust", "elevator", "aileron", "rudder"});
+                    + std::to_string(parent_->id().id()) + "-states.csv"
+             << endl;
+
+        csv_.set_column_headers(sc::CSV::Headers{
+            "t",     "x",         "y",           "z",           "U",           "V",     "W",
+            "alpha", "alpha_dot", "beta",        "P",           "Q",           "R",     "Ax_b",
+            "Ay_b",  "Az_b",      "AngAccelx_b", "AngAccely_b", "AngAccelz_b", "roll",  "pitch",
+            "yaw",   "throttle",  "thrust",      "elevator",    "aileron",     "rudder"});
     }
 
-    rho_ = sc::get<double>("air_density", params, rho_); // air density
-    wind_(0) = sc::get<double>("wind_E", params, 0); // wind constant
-    wind_(1) = sc::get<double>("wind_N", params, 0); // wind constant
-    wind_(2) = sc::get<double>("wind_U", params, 0); // wind constant
+    rho_ = sc::get<double>("air_density", params, rho_);  // air density
+    wind_(0) = sc::get<double>("wind_E", params, 0);      // wind constant
+    wind_(1) = sc::get<double>("wind_N", params, 0);      // wind constant
+    wind_(2) = sc::get<double>("wind_U", params, 0);      // wind constant
 
     // thrust and dimensional specs
     throttle_input_min_ = sc::get<double>("throttle_input_min", params, throttle_input_min_);
@@ -223,7 +215,7 @@ bool FixedWing6DOF::init(std::map<std::string, std::string> &info,
     S_ = sc::get<double>("surface_area_of_wing", params, S_);
     c_ = sc::get<double>("chord_length", params, c_);
     e_ = sc::get<double>("efficiency_factor", params, e_);
-    AR_ = b_/c_;
+    AR_ = b_ / c_;
 
     // Drag coefficients
     C_D0_ = sc::get<double>("C_D0", params, C_D0_);
@@ -268,8 +260,8 @@ bool FixedWing6DOF::init(std::map<std::string, std::string> &info,
 bool FixedWing6DOF::step(double time, double dt) {
     // Get inputs and saturate
     throttle_ = clamp(vars_.input(throttle_idx_), -1.0, 1.0);
-    thrust_ = scale<double>(throttle_, throttle_input_min_,
-                            throttle_input_max_, thrust_min_, thrust_max_);
+    thrust_ = scale<double>(throttle_, throttle_input_min_, throttle_input_max_, thrust_min_,
+                            thrust_max_);
 
     delta_elevator_ = clamp(vars_.input(elevator_idx_), delta_elevator_min_, delta_elevator_max_);
     delta_aileron_ = clamp(vars_.input(aileron_idx_), delta_aileron_min_, delta_aileron_max_);
@@ -285,7 +277,8 @@ bool FixedWing6DOF::step(double time, double dt) {
     x_[V] = local_lin_vel(1);
     x_[W] = local_lin_vel(2);
 
-    Eigen::Vector3d local_ang_vel(state_->ang_vel()(0), -state_->ang_vel()(1), -state_->ang_vel()(2));
+    Eigen::Vector3d local_ang_vel(state_->ang_vel()(0), -state_->ang_vel()(1),
+                                  -state_->ang_vel()(2));
     local_ang_vel = quat_body_.rotate_reverse(local_ang_vel);
 
     x_[P] = local_ang_vel(0);
@@ -319,42 +312,41 @@ bool FixedWing6DOF::step(double time, double dt) {
     // wind frame velocity
     Eigen::Vector3d wind_B(wind_(0), -wind_(1), -wind_(2));
     wind_B = quat_body_.rotate_reverse(wind_B);
-    Eigen::Vector3d vel_rel_wind(x_[U]+wind_B(0), x_[V]+wind_B(1), x_[W]+wind_B(2));
+    Eigen::Vector3d vel_rel_wind(x_[U] + wind_B(0), x_[V] + wind_B(1), x_[W] + wind_B(2));
 
-    alpha_ = atan2(vel_rel_wind(2), vel_rel_wind(0)); // angle of attack
+    alpha_ = atan2(vel_rel_wind(2), vel_rel_wind(0));  // angle of attack
     // TODO: Need to compute alpha_dot without using alpha_prev_
     alpha_dot_ = 0;
     // alpha_dot_ = (alpha_ - alpha_prev_) / dt;
     // alpha_prev_ = alpha_;
 
-    if (time > launch_time_)
-        launch_command_ = 1;
+    if (time > launch_time_) launch_command_ = 1;
 
     // Eigen::Vector3d F_catapult_NED(0, 0, 0);
     Eigen::Vector3d F_catapult(0, 0, 0);
 
-    double launch_duration = launch_speed_/launch_accel_;
+    double launch_duration = launch_speed_ / launch_accel_;
     switch (launch_state_) {
-    case PRELAUNCH:
-        if (launch_command_) {
-            launch_state_ = LAUNCH;
-            launch_start_t_ = time;
-        }
-        break;
-    case LAUNCH:
-        F_catapult(0) = launch_accel_ * mass_;
-        if (launch_duration < time - launch_start_t_) {
-            launch_state_ = POSTLAUNCH;
-        }
-        break;
-    case POSTLAUNCH:
-        // normal operation
-        break;
+        case PRELAUNCH:
+            if (launch_command_) {
+                launch_state_ = LAUNCH;
+                launch_start_t_ = time;
+            }
+            break;
+        case LAUNCH:
+            F_catapult(0) = launch_accel_ * mass_;
+            if (launch_duration < time - launch_start_t_) {
+                launch_state_ = POSTLAUNCH;
+            }
+            break;
+        case POSTLAUNCH:
+            // normal operation
+            break;
     }
 
     // Apply any external forces (todo)
     Eigen::Vector3d ext_force_NED(ext_force_(0), -ext_force_(1), -ext_force_(2));
-    ext_force_ = Eigen::Vector3d::Zero(); // reset ext_force_ member variable
+    ext_force_ = Eigen::Vector3d::Zero();  // reset ext_force_ member variable
     force_ext_body_ = quat_body_.rotate_reverse(ext_force_NED);
     force_ext_body_ += F_catapult;
 
@@ -412,76 +404,73 @@ bool FixedWing6DOF::step(double time, double dt) {
     }
 
     if (write_csv_) {
-        double beta = atan2(x_[V], x_[U]); // side slip
+        double beta = atan2(x_[V], x_[U]);  // side slip
         // Log state to CSV
-        csv_.append(sc::CSV::Pairs{
-                {"t", time},
-                {"x", x_[Xw]},
-                {"y", x_[Yw]},
-                {"z", x_[Zw]},
-                {"U", x_[U]},
-                {"V", x_[V]},
-                {"W", x_[W]},
-                {"alpha", alpha_},
-                {"alpha_dot", alpha_dot_},
-                {"beta", beta},
-                {"P", x_[P]},
-                {"Q", x_[Q]},
-                {"R", x_[R]},
-                {"Ax_b", linear_accel_body_(0)},
-                {"Ay_b", linear_accel_body_(1)},
-                {"Az_b", linear_accel_body_(2)},
-                {"AngAccelx_b", ang_accel_body_(0)},
-                {"AngAccely_b", ang_accel_body_(1)},
-                {"AngAccelz_b", ang_accel_body_(2)},
-                {"roll", quat_body_.roll()},
-                {"pitch", quat_body_.pitch()},
-                {"yaw", quat_body_.yaw()},
-                {"throttle", throttle_},
-                {"thrust", thrust_},
-                {"elevator", delta_elevator_},
-                {"aileron", delta_aileron_},
-                {"rudder", delta_rudder_}});
+        csv_.append(sc::CSV::Pairs{{"t", time},
+                                   {"x", x_[Xw]},
+                                   {"y", x_[Yw]},
+                                   {"z", x_[Zw]},
+                                   {"U", x_[U]},
+                                   {"V", x_[V]},
+                                   {"W", x_[W]},
+                                   {"alpha", alpha_},
+                                   {"alpha_dot", alpha_dot_},
+                                   {"beta", beta},
+                                   {"P", x_[P]},
+                                   {"Q", x_[Q]},
+                                   {"R", x_[R]},
+                                   {"Ax_b", linear_accel_body_(0)},
+                                   {"Ay_b", linear_accel_body_(1)},
+                                   {"Az_b", linear_accel_body_(2)},
+                                   {"AngAccelx_b", ang_accel_body_(0)},
+                                   {"AngAccely_b", ang_accel_body_(1)},
+                                   {"AngAccelz_b", ang_accel_body_(2)},
+                                   {"roll", quat_body_.roll()},
+                                   {"pitch", quat_body_.pitch()},
+                                   {"yaw", quat_body_.yaw()},
+                                   {"throttle", throttle_},
+                                   {"thrust", thrust_},
+                                   {"elevator", delta_elevator_},
+                                   {"aileron", delta_aileron_},
+                                   {"rudder", delta_rudder_}});
     }
     return true;
 }
 
-void FixedWing6DOF::model(const vector_t &x , vector_t &dxdt , double t) {
+void FixedWing6DOF::model(const vector_t& x, vector_t& dxdt, double t) {
     // Calculate velocity magnitude (handle zero velocity)
     Eigen::Vector3d wind_B(wind_(0), -wind_(1), -wind_(2));
     wind_B = quat_body_.rotate_reverse(wind_B);
-    Eigen::Vector3d vel_rel_wind(x_[U]+wind_B(0), x_[V]+wind_B(1), x_[W]+wind_B(2));
+    Eigen::Vector3d vel_rel_wind(x_[U] + wind_B(0), x_[V] + wind_B(1), x_[W] + wind_B(2));
     double V_tau = vel_rel_wind.norm();
     if (std::abs(V_tau) < std::numeric_limits<double>::epsilon()) {
         V_tau = 0.00001;
     }
 
     // Calculate commonly used quantities
-    double beta = asin(clamp(vel_rel_wind(1)/V_tau, -1.0, 1.0)); // side slip
+    double beta = asin(clamp(vel_rel_wind(1) / V_tau, -1.0, 1.0));  // side slip
     // double delta_Ve = 0.05*V_tau; // wind velocity across tail of aircraft (approximation)
     // double VtauVe = pow((V_tau + delta_Ve)/V_tau, 2);
     double pVtS = rho_ * pow(V_tau, 2) * S_ / 2.0;
 
-
     // Calculate lift, drag, and side_force magnitudes
-    double CL = (C_L0_ + C_L_alpha_*alpha_ + C_LQ_*x_[Q] * c_ / (2*V_tau) +
-                   C_L_alpha_dot_ * alpha_dot_ * c_ / (2*V_tau) +
-                   C_L_delta_elevator_ * delta_elevator_);
+    double CL =
+        (C_L0_ + C_L_alpha_ * alpha_ + C_LQ_ * x_[Q] * c_ / (2 * V_tau)
+         + C_L_alpha_dot_ * alpha_dot_ * c_ / (2 * V_tau) + C_L_delta_elevator_ * delta_elevator_);
     double lift = CL * pVtS;
 
-    double drag = (C_D0_ + C_D_alpha_*std::abs(alpha_) + CL*CL/(M_PI*AR_*e_) +
-                   C_D_delta_elevator_*std::abs(delta_elevator_)) * pVtS;
-
+    double drag = (C_D0_ + C_D_alpha_ * std::abs(alpha_) + CL * CL / (M_PI * AR_ * e_)
+                   + C_D_delta_elevator_ * std::abs(delta_elevator_))
+                  * pVtS;
 
-    double side_force = (C_Y_beta_*beta + C_Y_delta_rudder_*delta_rudder_) * pVtS;
+    double side_force = (C_Y_beta_ * beta + C_Y_delta_rudder_ * delta_rudder_) * pVtS;
 
     // Bring lift, drag, side_force magnitudes into body frame
-    Eigen::Vector3d F_aero(lift*sin(alpha_) - drag*cos(alpha_) - side_force*sin(beta),
-                           side_force*cos(beta),
-                           -lift*cos(alpha_) - drag*sin(alpha_));
+    Eigen::Vector3d F_aero(lift * sin(alpha_) - drag * cos(alpha_) - side_force * sin(beta),
+                           side_force * cos(beta), -lift * cos(alpha_) - drag * sin(alpha_));
 
     // Calculate force from weight in body frame:
-    Eigen::Vector3d gravity_vector(0, 0, +mass_*g_);
+    Eigen::Vector3d gravity_vector(0, 0, +mass_ * g_);
     Eigen::Vector3d F_weight = quat_body_.rotate_reverse(gravity_vector);
 
     Eigen::Vector3d F_thrust(thrust_, 0, 0);
@@ -492,12 +481,12 @@ void FixedWing6DOF::model(const vector_t &x , vector_t &dxdt , double t) {
     Eigen::Vector3d F_ground(0, 0, 0);
     if (x_[Zw] < 0 && use_ground_model_) {
         double wn = 20;
-        double Kp = wn*wn*mass_;
-        double Kd = 2*wn;
+        double Kp = wn * wn * mass_;
+        double Kd = 2 * wn;
 
-        F_ground_W[2] = std::max(0.0, -Kp*(x_[Zw] - 0.0) - Kd*(x_[Ww] - 0.0));
+        F_ground_W[2] = std::max(0.0, -Kp * (x_[Zw] - 0.0) - Kd * (x_[Ww] - 0.0));
 
-        F_ground(0) =  F_ground_W(0);
+        F_ground(0) = F_ground_W(0);
         F_ground(1) = -F_ground_W(1);
         F_ground(2) = -F_ground_W(2);
         F_ground = quat_body_.rotate_reverse(F_ground);
@@ -505,28 +494,35 @@ void FixedWing6DOF::model(const vector_t &x , vector_t &dxdt , double t) {
         F_total += F_ground;
     }
     if (POSTLAUNCH != launch_state_) {
-        F_total = Eigen::Vector3d::Zero();;
+        F_total = Eigen::Vector3d::Zero();
+        ;
     }
     F_total += force_ext_body_;
 
     // Calculate body frame linear velocities
-    dxdt[U] = x[V]*x[R] - x[W]*x[Q] + F_total(0) / mass_;
-    dxdt[V] = x[W]*x[P] - x[U]*x[R] + F_total(1) / mass_;
-    dxdt[W] = x[U]*x[Q] - x[V]*x[P] + F_total(2) / mass_;
+    dxdt[U] = x[V] * x[R] - x[W] * x[Q] + F_total(0) / mass_;
+    dxdt[V] = x[W] * x[P] - x[U] * x[R] + F_total(1) / mass_;
+    dxdt[W] = x[U] * x[Q] - x[V] * x[P] + F_total(2) / mass_;
 
     // Calculate moments;
-    Eigen::Vector3d Moments_thrust(0, 0, 0); // no moment from thrust
-    Eigen::Vector3d Moments_torque(0, 0, 0); // no moment from torque
-    Eigen::Vector3d Moments_gyro(0, 0, 0);   // no moment from gyro effect
+    Eigen::Vector3d Moments_thrust(0, 0, 0);  // no moment from thrust
+    Eigen::Vector3d Moments_torque(0, 0, 0);  // no moment from torque
+    Eigen::Vector3d Moments_gyro(0, 0, 0);    // no moment from gyro effect
 
     // Calculate moments from aerodynamic forces
-    Eigen::Vector3d Moments_aero((C_L_beta_*beta + C_LP_*x_[P]*b_/(2*V_tau) + C_LR_*x_[R]*b_/(2*V_tau) + C_L_delta_aileron_*delta_aileron_ + C_L_delta_rudder_*delta_rudder_) * pVtS*b_,
-                                 (C_M0_ + C_M_alpha_*alpha_ + C_MQ_*x_[Q]*c_/(2*V_tau) + C_M_alpha_dot_*alpha_dot_*c_/(2*V_tau) + C_M_delta_elevator_*delta_elevator_) * pVtS*c_,
-                                 (C_N_beta_*beta + C_NP_*x_[P]*b_/(2*V_tau) + C_NR_*x_[R]*b_/(2*V_tau) + C_N_delta_aileron_*delta_aileron_ + C_N_delta_rudder_*delta_rudder_) * pVtS*b_);
+    Eigen::Vector3d Moments_aero(
+        (C_L_beta_ * beta + C_LP_ * x_[P] * b_ / (2 * V_tau) + C_LR_ * x_[R] * b_ / (2 * V_tau)
+         + C_L_delta_aileron_ * delta_aileron_ + C_L_delta_rudder_ * delta_rudder_)
+            * pVtS * b_,
+        (C_M0_ + C_M_alpha_ * alpha_ + C_MQ_ * x_[Q] * c_ / (2 * V_tau)
+         + C_M_alpha_dot_ * alpha_dot_ * c_ / (2 * V_tau) + C_M_delta_elevator_ * delta_elevator_)
+            * pVtS * c_,
+        (C_N_beta_ * beta + C_NP_ * x_[P] * b_ / (2 * V_tau) + C_NR_ * x_[R] * b_ / (2 * V_tau)
+         + C_N_delta_aileron_ * delta_aileron_ + C_N_delta_rudder_ * delta_rudder_)
+            * pVtS * b_);
 
     // Sum moments
-    Eigen::Vector3d Moments_total = Moments_aero + Moments_thrust +
-        Moments_torque + Moments_gyro;
+    Eigen::Vector3d Moments_total = Moments_aero + Moments_thrust + Moments_torque + Moments_gyro;
 
     if (POSTLAUNCH != launch_state_) {
         Moments_total = Eigen::Vector3d::Zero();
@@ -534,17 +530,17 @@ void FixedWing6DOF::model(const vector_t &x , vector_t &dxdt , double t) {
 
     // Calculate rotational velocites
     Eigen::Vector3d pqr(x_[P], x_[Q], x_[R]);
-    Eigen::Vector3d pqr_dot = I_inv_ * (Moments_total - pqr.cross(I_*pqr));
+    Eigen::Vector3d pqr_dot = I_inv_ * (Moments_total - pqr.cross(I_ * pqr));
     dxdt[P] = pqr_dot(0);
     dxdt[Q] = pqr_dot(1);
     dxdt[R] = pqr_dot(2);
 
     // Compute quaternion derivatives
     double lambda = 1 - (pow(x[q0], 2) + pow(x[q1], 2) + pow(x[q2], 2) + pow(x[q3], 2));
-    dxdt[q0] = -0.5 * (x[q1]*x[P] + x[q2]*x[Q] + x[q3]*x[R]) + lambda * x[q0];
-    dxdt[q1] = +0.5 * (x[q0]*x[P] + x[q2]*x[R] - x[q3]*x[Q]) + lambda * x[q1];
-    dxdt[q2] = +0.5 * (x[q0]*x[Q] + x[q3]*x[P] - x[q1]*x[R]) + lambda * x[q2];
-    dxdt[q3] = +0.5 * (x[q0]*x[R] + x[q1]*x[Q] - x[q2]*x[P]) + lambda * x[q3];
+    dxdt[q0] = -0.5 * (x[q1] * x[P] + x[q2] * x[Q] + x[q3] * x[R]) + lambda * x[q0];
+    dxdt[q1] = +0.5 * (x[q0] * x[P] + x[q2] * x[R] - x[q3] * x[Q]) + lambda * x[q1];
+    dxdt[q2] = +0.5 * (x[q0] * x[Q] + x[q3] * x[P] - x[q1] * x[R]) + lambda * x[q2];
+    dxdt[q3] = +0.5 * (x[q0] * x[R] + x[q1] * x[Q] - x[q2] * x[P]) + lambda * x[q3];
 
     // Normalize quaternion
     sc::Quaternion quat(x[q0], x[q1], x[q2], x[q3]);
@@ -552,20 +548,20 @@ void FixedWing6DOF::model(const vector_t &x , vector_t &dxdt , double t) {
 
     // Integrate local velocities to compute local positions
     Eigen::Vector3d vel_local(x[U], x[V], x[W]);
-    Eigen::Vector3d vel_world = quat.rotate(vel_local); // rot * vel_local;
+    Eigen::Vector3d vel_world = quat.rotate(vel_local);  // rot * vel_local;
     dxdt[Xw] = vel_world(0);
-    dxdt[Yw] = -vel_world(1); // Due to rotated frame
-    dxdt[Zw] = -vel_world(2); // Due to rotated frame
+    dxdt[Yw] = -vel_world(1);  // Due to rotated frame
+    dxdt[Zw] = -vel_world(2);  // Due to rotated frame
 
     // Integrate local accelerations to compute global velocities
     Eigen::Vector3d acc_local = F_total / mass_;
-    Eigen::Vector3d acc_world = quat.rotate(acc_local); // rot * acc_local;
+    Eigen::Vector3d acc_world = quat.rotate(acc_local);  // rot * acc_local;
     dxdt[Uw] = acc_world(0);
-    dxdt[Vw] = -acc_world(1); // Due to rotated frame
-    dxdt[Ww] = -acc_world(2); // Due to rotated frame
+    dxdt[Vw] = -acc_world(1);  // Due to rotated frame
+    dxdt[Ww] = -acc_world(2);  // Due to rotated frame
 }
 
-void FixedWing6DOF::teleport(StatePtr &state) {
+void FixedWing6DOF::teleport(StatePtr& state) {
     state_->pos() = state->pos();
     state_->vel() = state->vel();
     state_->ang_vel() = state->ang_vel();
@@ -573,5 +569,5 @@ void FixedWing6DOF::teleport(StatePtr &state) {
     skip_propagation_ = true;
 }
 
-} // namespace motion
-} // namespace scrimmage
+}  // namespace motion
+}  // namespace scrimmage
diff --git a/src/plugins/motion/HarmonicOscillator/HarmonicOscillator.cpp b/src/plugins/motion/HarmonicOscillator/HarmonicOscillator.cpp
index f28a7cca16..fcbfe50a27 100644
--- a/src/plugins/motion/HarmonicOscillator/HarmonicOscillator.cpp
+++ b/src/plugins/motion/HarmonicOscillator/HarmonicOscillator.cpp
@@ -29,40 +29,36 @@
  *
  */
 
-#include <scrimmage/plugins/motion/HarmonicOscillator/HarmonicOscillator.h>
 #include <scrimmage/common/Utilities.h>
-#include <scrimmage/parse/ParseUtils.h>
-#include <scrimmage/math/Angles.h>
-#include <scrimmage/plugin_manager/RegisterPlugin.h>
+#include <scrimmage/common/VariableIO.h>
 #include <scrimmage/entity/Entity.h>
+#include <scrimmage/math/Angles.h>
 #include <scrimmage/math/State.h>
-#include <scrimmage/common/VariableIO.h>
+#include <scrimmage/parse/ParseUtils.h>
+#include <scrimmage/plugin_manager/RegisterPlugin.h>
+#include <scrimmage/plugins/motion/HarmonicOscillator/HarmonicOscillator.h>
 
 #include <cmath>
 
 namespace sc = scrimmage;
 
-REGISTER_PLUGIN(scrimmage::MotionModel,
-                scrimmage::motion::HarmonicOscillator,
+REGISTER_PLUGIN(scrimmage::MotionModel, scrimmage::motion::HarmonicOscillator,
                 HarmonicOscillator_plugin)
 
 namespace scrimmage {
 namespace motion {
 
-enum ModelParams {
-    Z = 0,
-    VZ,
-    MODEL_NUM_ITEMS
-};
+enum ModelParams { Z = 0, VZ, MODEL_NUM_ITEMS };
 
-bool HarmonicOscillator::init(std::map<std::string, std::string> &info,
-                     std::map<std::string, std::string> &params) {
+bool HarmonicOscillator::init(std::map<std::string, std::string>& info,
+                              std::map<std::string, std::string>& params) {
     // params
     damping_ratio = sc::get<double>("damping_ratio", params, 0.707);
     natural_frequency = sc::get<double>("natural_frequency", params, 1);
 
     // Declare variables for controllers
-    acceleration_z_idx_ = vars_.declare(VariableIO::Type::acceleration_z, VariableIO::Direction::In);
+    acceleration_z_idx_ =
+        vars_.declare(VariableIO::Type::acceleration_z, VariableIO::Direction::In);
 
     x_.resize(MODEL_NUM_ITEMS);
     x_[Z] = state_->pos()(2);
@@ -72,7 +68,6 @@ bool HarmonicOscillator::init(std::map<std::string, std::string> &info,
 }
 
 bool HarmonicOscillator::step(double time, double dt) {
-
     ode_step(dt);
     state_->pos()(2) = x_[Z];
     state_->vel()(2) = x_[VZ];
@@ -80,12 +75,12 @@ bool HarmonicOscillator::step(double time, double dt) {
     return true;
 }
 
-void HarmonicOscillator::model(const vector_t &x , vector_t &dxdt ,
-                                double t) {
+void HarmonicOscillator::model(const vector_t& x, vector_t& dxdt, double t) {
     const double acc = vars_.input(acceleration_z_idx_);
 
     dxdt[Z] = x[VZ];
-    dxdt[VZ] = -std::pow(natural_frequency, 2) * x[Z] - 2 * damping_ratio * natural_frequency * x[VZ] + acc;
+    dxdt[VZ] = -std::pow(natural_frequency, 2) * x[Z]
+               - 2 * damping_ratio * natural_frequency * x[VZ] + acc;
 }
-} // namespace motion
-} // namespace scrimmage
+}  // namespace motion
+}  // namespace scrimmage
diff --git a/src/plugins/motion/JSBSimControl/JSBSimControl.cpp b/src/plugins/motion/JSBSimControl/JSBSimControl.cpp
index 54a3a20c74..c809a2793d 100644
--- a/src/plugins/motion/JSBSimControl/JSBSimControl.cpp
+++ b/src/plugins/motion/JSBSimControl/JSBSimControl.cpp
@@ -30,18 +30,17 @@
  *
  */
 
-#include <scrimmage/plugins/motion/JSBSimControl/JSBSimControl.h>
-#include <scrimmage/plugins/motion/JSBSimModel/FGOutputFGMod.h>
-
 #include <scrimmage/common/Utilities.h>
-#include <scrimmage/parse/ParseUtils.h>
-#include <scrimmage/parse/MissionParse.h>
+#include <scrimmage/entity/Entity.h>
 #include <scrimmage/math/Angles.h>
 #include <scrimmage/math/State.h>
-#include <scrimmage/entity/Entity.h>
+#include <scrimmage/parse/MissionParse.h>
+#include <scrimmage/parse/ParseUtils.h>
 #include <scrimmage/plugin_manager/RegisterPlugin.h>
-#include <scrimmage/proto/Shape.pb.h>
+#include <scrimmage/plugins/motion/JSBSimControl/JSBSimControl.h>
+#include <scrimmage/plugins/motion/JSBSimModel/FGOutputFGMod.h>
 #include <scrimmage/proto/ProtoConversions.h>
+#include <scrimmage/proto/Shape.pb.h>
 
 // c system
 #include <JSBSim/initialization/FGTrim.h>
@@ -51,12 +50,12 @@
 #include <iostream>
 
 // other
-#include <JSBSim/simgear/misc/sg_path.hxx>
 #include <GeographicLib/LocalCartesian.hpp>
+#include <JSBSim/simgear/misc/sg_path.hxx>
 #include <boost/algorithm/clamp.hpp>
 
-using std::cout;
 using std::cerr;
+using std::cout;
 using std::endl;
 
 #define meters2feet 3.28084
@@ -88,8 +87,8 @@ std::tuple<int, int, int> JSBSimControl::version() {
     return std::tuple<int, int, int>(0, 0, 1);
 }
 
-bool JSBSimControl::init(std::map<std::string, std::string> &info,
-                         std::map<std::string, std::string> &params) {
+bool JSBSimControl::init(std::map<std::string, std::string>& info,
+                         std::map<std::string, std::string>& params) {
     draw_vel_ = sc::get<double>("drawVel", params, 1.0);
     draw_ang_vel_ = sc::get<double>("drawAngVel", params, 10.0);
     draw_acc_ = sc::get<double>("drawAcc", params, 1.0);
@@ -108,7 +107,7 @@ bool JSBSimControl::init(std::map<std::string, std::string> &info,
         std::string ip = get<std::string>("flightgear_ip", params, "localhost");
         std::string port = get<std::string>("flightgear_port", params, "5600");
         std::string protocol = get<std::string>("flightgear_protocol", params, "UDP");
-        std::string name = ip + ":" + protocol + "/" + port; // localhost:UDP/5600
+        std::string name = ip + ":" + protocol + "/" + port;  // localhost:UDP/5600
 
         output_fg_->SetIdx(0);
         output_fg_->SetOutputName(name);
@@ -128,11 +127,11 @@ bool JSBSimControl::init(std::map<std::string, std::string> &info,
     exec_->SetEnginePath(SGPath("engine"));
     exec_->SetSystemsPath(SGPath("systems"));
 
-    exec_->LoadScript(SGPath("scripts/"+info["script_name"]));
+    exec_->LoadScript(SGPath("scripts/" + info["script_name"]));
 
-    JSBSim::FGInitialCondition *ic = exec_->GetIC();
+    JSBSim::FGInitialCondition* ic = exec_->GetIC();
 
-    Quaternion q_ned_enu(M_PI, 0.0, M_PI/2.0);
+    Quaternion q_ned_enu(M_PI, 0.0, M_PI / 2.0);
     Quaternion q_flu_frd(M_PI, 0.0, 0.0);
     Quaternion q_frd_enu(q_ned_enu * state_->quat() * q_flu_frd);
     ic->SetPsiRadIC(q_frd_enu.yaw());
@@ -146,7 +145,8 @@ bool JSBSimControl::init(std::map<std::string, std::string> &info,
     ic->SetTerrainElevationFtIC(parent_->projection()->HeightOrigin() * meters2feet);
 
     Eigen::Vector3d lla;
-    parent_->projection()->Reverse(state_->pos()[0], state_->pos()[1], state_->pos()[2], lla[0], lla[1], lla[2]);
+    parent_->projection()->Reverse(state_->pos()[0], state_->pos()[1], state_->pos()[2], lla[0],
+                                   lla[1], lla[2]);
     ic->SetLatitudeDegIC(lla[0]);
     ic->SetLongitudeDegIC(lla[1]);
     ic->SetAltitudeASLFtIC(lla[2] * meters2feet);
@@ -191,7 +191,7 @@ bool JSBSimControl::init(std::map<std::string, std::string> &info,
     }
 
     exec_->RunIC();
-    exec_->Setdt(std::stod(info["dt"])/std::stod(info["motion_multiplier"]));
+    exec_->Setdt(std::stod(info["dt"]) / std::stod(info["motion_multiplier"]));
     exec_->Run();
 
     // Get references to each of the nodes that hold properties that we
@@ -217,7 +217,6 @@ bool JSBSimControl::init(std::map<std::string, std::string> &info,
 
     u_vel_node_ = mgr->GetNode("velocities/u-fps");
 
-
     // angular velocity in ECEF frame
     p_node_ = mgr->GetNode("velocities/p-rad_sec");
     q_node_ = mgr->GetNode("velocities/q-rad_sec");
@@ -228,46 +227,40 @@ bool JSBSimControl::init(std::map<std::string, std::string> &info,
     ay_pilot_node_ = mgr->GetNode("accelerations/a-pilot-y-ft_sec2");
     az_pilot_node_ = mgr->GetNode("accelerations/a-pilot-z-ft_sec2");
 
-
     // Save state
     parent_->projection()->Forward(latitude_node_->getDoubleValue(),
-                                  longitude_node_->getDoubleValue(),
-                                  altitude_node_->getDoubleValue() * feet2meters,
-                                  state_->pos()(0), state_->pos()(1), state_->pos()(2));
+                                   longitude_node_->getDoubleValue(),
+                                   altitude_node_->getDoubleValue() * feet2meters, state_->pos()(0),
+                                   state_->pos()(1), state_->pos()(2));
 
     angles_from_jsbsim_.set_angle(ang::rad2deg(yaw_node_->getDoubleValue()));
 
-    state_->quat().set(roll_node_->getDoubleValue(),
-                      -pitch_node_->getDoubleValue(),
-                      ang::deg2rad(angles_from_jsbsim_.angle()));
+    state_->quat().set(roll_node_->getDoubleValue(), -pitch_node_->getDoubleValue(),
+                       ang::deg2rad(angles_from_jsbsim_.angle()));
 
     state_->vel() << vel_east_node_->getDoubleValue() * feet2meters,
         vel_north_node_->getDoubleValue() * feet2meters,
         -vel_down_node_->getDoubleValue() * feet2meters;
 
-    Eigen::Vector3d ang_vel_FLU(p_node_->getDoubleValue(),
-                               -q_node_->getDoubleValue(),
-                               -r_node_->getDoubleValue());
+    Eigen::Vector3d ang_vel_FLU(p_node_->getDoubleValue(), -q_node_->getDoubleValue(),
+                                -r_node_->getDoubleValue());
     state_->ang_vel() = state_->quat().rotate(ang_vel_FLU);
 
-    Eigen::Vector3d a_FLU(ax_pilot_node_->getDoubleValue(),
-                         -ay_pilot_node_->getDoubleValue(),
-                         -az_pilot_node_->getDoubleValue());
+    Eigen::Vector3d a_FLU(ax_pilot_node_->getDoubleValue(), -ay_pilot_node_->getDoubleValue(),
+                          -az_pilot_node_->getDoubleValue());
     linear_accel_body_ = state_->quat().rotate(a_FLU);
 
     return true;
 }
 
 bool JSBSimControl::step(double time, double dt) {
-
-    throttle_       = ba::clamp(vars_.input(throttle_idx_), -1.0, 1.0);
+    throttle_ = ba::clamp(vars_.input(throttle_idx_), -1.0, 1.0);
     delta_elevator_ = ba::clamp(vars_.input(elevator_idx_), -1.0, 1.0);
-    delta_aileron_  = ba::clamp(vars_.input(aileron_idx_),  -1.0, 1.0);
-    delta_rudder_   = ba::clamp(vars_.input(rudder_idx_),   -1.0, 1.0);
+    delta_aileron_ = ba::clamp(vars_.input(aileron_idx_), -1.0, 1.0);
+    delta_rudder_ = ba::clamp(vars_.input(rudder_idx_), -1.0, 1.0);
 
     // TODO: for some reason, jsb sim does not like it when there is an immediate thottle input
-    if (time < .05)
-        throttle_ = 0;
+    if (time < .05) throttle_ = 0;
 
     ap_aileron_cmd_node_->setDoubleValue(delta_aileron_);
     ap_elevator_cmd_node_->setDoubleValue(delta_elevator_);
@@ -277,36 +270,29 @@ bool JSBSimControl::step(double time, double dt) {
     exec_->Setdt(dt);
     exec_->Run();
 
-
-
-
     ///////////////////////////////////////////////////////////////////////////
     // Save state
     parent_->projection()->Forward(latitude_node_->getDoubleValue(),
-                                  longitude_node_->getDoubleValue(),
-                                  altitude_node_->getDoubleValue() * feet2meters,
-                                  state_->pos()(0), state_->pos()(1), state_->pos()(2));
+                                   longitude_node_->getDoubleValue(),
+                                   altitude_node_->getDoubleValue() * feet2meters, state_->pos()(0),
+                                   state_->pos()(1), state_->pos()(2));
 
     angles_from_jsbsim_.set_angle(ang::rad2deg(yaw_node_->getDoubleValue()));
 
-    state_->quat().set(roll_node_->getDoubleValue(),
-                      -pitch_node_->getDoubleValue(),
-                      ang::deg2rad(angles_from_jsbsim_.angle()));
-
+    state_->quat().set(roll_node_->getDoubleValue(), -pitch_node_->getDoubleValue(),
+                       ang::deg2rad(angles_from_jsbsim_.angle()));
 
     state_->vel() << vel_east_node_->getDoubleValue() * feet2meters,
         vel_north_node_->getDoubleValue() * feet2meters,
         -vel_down_node_->getDoubleValue() * feet2meters;
 
-
-    Eigen::Vector3d ang_vel_FLU(p_node_->getDoubleValue(),
-                               -q_node_->getDoubleValue(),
-                               -r_node_->getDoubleValue());
+    Eigen::Vector3d ang_vel_FLU(p_node_->getDoubleValue(), -q_node_->getDoubleValue(),
+                                -r_node_->getDoubleValue());
     state_->ang_vel() = state_->quat().rotate(ang_vel_FLU);
 
     Eigen::Vector3d a_FLU(ax_pilot_node_->getDoubleValue() * feet2meters,
-                         -ay_pilot_node_->getDoubleValue() * feet2meters,
-                         -az_pilot_node_->getDoubleValue() * feet2meters);
+                          -ay_pilot_node_->getDoubleValue() * feet2meters,
+                          -az_pilot_node_->getDoubleValue() * feet2meters);
     // TODO: jsbsim returns specific force, but need to populate this value with
     // acceleration. Need to make gravity not a hard-coded value or find a better
     // way to handle this.
@@ -390,5 +376,5 @@ bool JSBSimControl::step(double time, double dt) {
 
     return true;
 }
-} // namespace motion
-} // namespace scrimmage
+}  // namespace motion
+}  // namespace scrimmage
diff --git a/src/plugins/motion/JSBSimModel/FGOutputFGMod.cpp b/src/plugins/motion/JSBSimModel/FGOutputFGMod.cpp
index 4ab9cd3344..fdf52d0903 100644
--- a/src/plugins/motion/JSBSimModel/FGOutputFGMod.cpp
+++ b/src/plugins/motion/JSBSimModel/FGOutputFGMod.cpp
@@ -31,7 +31,6 @@
  */
 
 #include <scrimmage/plugins/motion/JSBSimModel/FGOutputFGMod.h>
-
 #include <stdio.h>
 #include <string.h>
 
@@ -39,11 +38,11 @@
 
 #include "models/FGAerodynamics.h"
 #include "models/FGAuxiliary.h"
-#include "models/FGPropulsion.h"
+#include "models/FGFCS.h"
+#include "models/FGGroundReactions.h"
 #include "models/FGMassBalance.h"
 #include "models/FGPropagate.h"
-#include "models/FGGroundReactions.h"
-#include "models/FGFCS.h"
+#include "models/FGPropulsion.h"
 #include "models/propulsion/FGPiston.h"
 #include "models/propulsion/FGTank.h"
 
@@ -52,19 +51,19 @@
 // some NOLINT and cppcheck suppressions.
 
 static const int endianTest = 1;
-#define isLittleEndian (*((char *) &endianTest ) != 0) // NOLINT
+#define isLittleEndian (*((char*)&endianTest) != 0)  // NOLINT
 
 namespace JSBSim {
 
 // TODO: Replace JSBSim's methods for network byte ordering
-static void htond(double &x) {
+static void htond(double& x) {
     if (isLittleEndian) {
-        int    *Double_Overlay;
-        int     Holding_Buffer;
+        int* Double_Overlay;
+        int Holding_Buffer;
 
         // Double_Overlay = (int *) &x;
         // cppcheck-suppress invalidPointerCast
-        Double_Overlay = reinterpret_cast<int *>(&x);
+        Double_Overlay = reinterpret_cast<int*>(&x);
         Holding_Buffer = Double_Overlay[0];
 
         Double_Overlay[0] = htonl(Double_Overlay[1]);
@@ -76,13 +75,13 @@ static void htond(double &x) {
 
 // TODO: Replace JSBSim's methods for network byte ordering
 // Float version
-static void htonf(float &x) {
+static void htonf(float& x) {
     if (isLittleEndian) {
-        int    *Float_Overlay;
-        int     Holding_Buffer;
+        int* Float_Overlay;
+        int Holding_Buffer;
 
         // cppcheck-suppress invalidPointerCast
-        Float_Overlay = reinterpret_cast<int *>(&x);
+        Float_Overlay = reinterpret_cast<int*>(&x);
         Holding_Buffer = Float_Overlay[0];
 
         Float_Overlay[0] = htonl(Holding_Buffer);
@@ -91,230 +90,250 @@ static void htonf(float &x) {
     }
 }
 
-FGOutputFGMod::FGOutputFGMod(FGFDMExec* fdmex) :
-    FGOutputFG(fdmex) {
+FGOutputFGMod::FGOutputFGMod(FGFDMExec* fdmex) : FGOutputFG(fdmex) {
     memset(&fgSockBufMod, 0x0, sizeof(fgSockBufMod));
 }
 
 void FGOutputFGMod::Print(void) {
-  int length = sizeof(fgSockBufMod);
+    int length = sizeof(fgSockBufMod);
 
-  if (socket == 0) return;
-  if (!socket->GetConnectStatus()) return;
+    if (socket == 0) return;
+    if (!socket->GetConnectStatus()) return;
 
-  SocketDataFillMod(&fgSockBufMod);
-  socket->Send((char *)&fgSockBufMod, length); // NOLINT
+    SocketDataFillMod(&fgSockBufMod);
+    socket->Send((char*)&fgSockBufMod, length);  // NOLINT
 }
 
 void FGOutputFGMod::SocketDataFillMod(FGNetFDM* net) {
-  unsigned int i;
-
-  // Version
-  net->version = FG_NET_FDM_VERSION;
-
-  // Positions
-  net->longitude = Propagate->GetLocation().GetLongitude(); //
-  net->latitude  = Propagate->GetLocation().GetLatitude(); // GeodLatitudeRad(); // geodetic (radians)
-  net->altitude  = Propagate->GetAltitudeASL()*0.3048; // altitude, above sea level (meters)
-  net->agl       = static_cast<float>(Propagate->GetDistanceAGL()*0.3048); // altitude, above ground level (meters)
-
-  net->phi       = static_cast<float>(Propagate->GetEuler(ePhi)); // roll (radians)
-  net->theta     = static_cast<float>(Propagate->GetEuler(eTht)); // pitch (radians)
-  net->psi       = static_cast<float>(Propagate->GetEuler(ePsi)); // yaw or true heading (radians)
-
-  net->alpha     = static_cast<float>(Auxiliary->Getalpha()); // angle of attack (radians)
-  net->beta      = static_cast<float>(Auxiliary->Getbeta()); // side slip angle (radians)
-
-  // Velocities
-  net->phidot     = static_cast<float>(Auxiliary->GetEulerRates(ePhi)); // roll rate (radians/sec)
-  net->thetadot   = static_cast<float>(Auxiliary->GetEulerRates(eTht)); // pitch rate (radians/sec)
-  net->psidot     = static_cast<float>(Auxiliary->GetEulerRates(ePsi)); // yaw rate (radians/sec)
-  net->vcas       = static_cast<float>(Auxiliary->GetVcalibratedKTS()); // VCAS, knots
-  net->climb_rate = static_cast<float>(Propagate->Gethdot());           // altitude rate, ft/sec
-  net->v_north    = static_cast<float>(Propagate->GetVel(eNorth));      // north vel in NED frame, fps
-  net->v_east     = static_cast<float>(Propagate->GetVel(eEast));       // east vel in NED frame, fps
-  net->v_down     = static_cast<float>(Propagate->GetVel(eDown));       // down vel in NED frame, fps
-  // ---ADD METHOD TO CALCULATE THESE TERMS---
-  net->v_body_u = static_cast<float>(Propagate->GetUVW(1)); // ECEF speed in body axis
-  net->v_body_v = static_cast<float>(Propagate->GetUVW(2)); // ECEF speed in body axis
-  net->v_body_w = static_cast<float>(Propagate->GetUVW(3)); // ECEF speed in body axis
-
-  // Accelerations
-  net->A_X_pilot   = static_cast<float>(Auxiliary->GetPilotAccel(1));    // X body accel, ft/s/s
-  net->A_Y_pilot   = static_cast<float>(Auxiliary->GetPilotAccel(2));    // Y body accel, ft/s/s
-  net->A_Z_pilot   = static_cast<float>(Auxiliary->GetPilotAccel(3));    // Z body accel, ft/s/s
-
-  // Stall
-  net->stall_warning = 0.0;  // 0.0 - 1.0 indicating the amount of stall
-  net->slip_deg    = static_cast<float>(Auxiliary->Getbeta(inDegrees));  // slip ball deflection, deg
-
-  // Engine status
-  if (Propulsion->GetNumEngines() > FGNetFDM::FG_MAX_ENGINES && FDMExec->GetSimTime() == 0.0)
-    cerr << "This vehicle has " << Propulsion->GetNumEngines() << " engines, but the current " << endl
-         << "version of FlightGear's FGNetFDM only supports " << FGNetFDM::FG_MAX_ENGINES << " engines." << endl
-         << "Only the first " << FGNetFDM::FG_MAX_ENGINES << " engines will be used." << endl;
-
-  net->num_engines = std::min(static_cast<unsigned int>(FGNetFDM::FG_MAX_ENGINES),
-                              Propulsion->GetNumEngines()); // Number of valid engines
-
-  for (i=0; i < net->num_engines; i++) {
-    if (Propulsion->GetEngine(i)->GetRunning())
-      net->eng_state[i] = 2;       // Engine state running
-    else if (Propulsion->GetEngine(i)->GetCranking())
-      net->eng_state[i] = 1;       // Engine state cranking
-    else
-      net->eng_state[i] = 0;       // Engine state off
-
-    switch (Propulsion->GetEngine(i)->GetType()) {
-    case (FGEngine::etRocket):
-        break;
-    case (FGEngine::etPiston):
-        net->rpm[i]       = static_cast<float>((static_cast<FGPiston *>(Propulsion->GetEngine(i))->getRPM()));
-        net->fuel_flow[i] = static_cast<float>(((static_cast<FGPiston *>(Propulsion->GetEngine(i))->getFuelFlow_gph())));
-        net->fuel_px[i]   = 0; // Fuel pressure, psi  (N/A in current model)
-        net->egt[i]       = static_cast<float>(((static_cast<FGPiston *>(Propulsion->GetEngine(i))->GetEGT())));
-        net->cht[i]       = static_cast<float>(((static_cast<FGPiston *>(Propulsion->GetEngine(i))->getCylinderHeadTemp_degF())));
-        net->mp_osi[i]    = static_cast<float>(((static_cast<FGPiston *>(Propulsion->GetEngine(i))->getManifoldPressure_inHg())));
-        net->oil_temp[i]  = static_cast<float>(((static_cast<FGPiston *>(Propulsion->GetEngine(i))->getOilTemp_degF())));
-        net->oil_px[i]    = static_cast<float>(((static_cast<FGPiston *>(Propulsion->GetEngine(i))->getOilPressure_psi())));
-        net->tit[i]       = 0; // Turbine Inlet Temperature  (N/A for piston)
-        break;
-    case (FGEngine::etTurbine):
-        break;
-    case (FGEngine::etTurboprop):
-        break;
-    case (FGEngine::etElectric):
-        break;
-    case (FGEngine::etUnknown):
-        break;
+    unsigned int i;
+
+    // Version
+    net->version = FG_NET_FDM_VERSION;
+
+    // Positions
+    net->longitude = Propagate->GetLocation().GetLongitude();  //
+    net->latitude =
+        Propagate->GetLocation().GetLatitude();  // GeodLatitudeRad(); // geodetic (radians)
+    net->altitude = Propagate->GetAltitudeASL() * 0.3048;  // altitude, above sea level (meters)
+    net->agl = static_cast<float>(Propagate->GetDistanceAGL()
+                                  * 0.3048);  // altitude, above ground level (meters)
+
+    net->phi = static_cast<float>(Propagate->GetEuler(ePhi));    // roll (radians)
+    net->theta = static_cast<float>(Propagate->GetEuler(eTht));  // pitch (radians)
+    net->psi = static_cast<float>(Propagate->GetEuler(ePsi));    // yaw or true heading (radians)
+
+    net->alpha = static_cast<float>(Auxiliary->Getalpha());  // angle of attack (radians)
+    net->beta = static_cast<float>(Auxiliary->Getbeta());    // side slip angle (radians)
+
+    // Velocities
+    net->phidot = static_cast<float>(Auxiliary->GetEulerRates(ePhi));    // roll rate (radians/sec)
+    net->thetadot = static_cast<float>(Auxiliary->GetEulerRates(eTht));  // pitch rate (radians/sec)
+    net->psidot = static_cast<float>(Auxiliary->GetEulerRates(ePsi));    // yaw rate (radians/sec)
+    net->vcas = static_cast<float>(Auxiliary->GetVcalibratedKTS());      // VCAS, knots
+    net->climb_rate = static_cast<float>(Propagate->Gethdot());          // altitude rate, ft/sec
+    net->v_north = static_cast<float>(Propagate->GetVel(eNorth));  // north vel in NED frame, fps
+    net->v_east = static_cast<float>(Propagate->GetVel(eEast));    // east vel in NED frame, fps
+    net->v_down = static_cast<float>(Propagate->GetVel(eDown));    // down vel in NED frame, fps
+    // ---ADD METHOD TO CALCULATE THESE TERMS---
+    net->v_body_u = static_cast<float>(Propagate->GetUVW(1));  // ECEF speed in body axis
+    net->v_body_v = static_cast<float>(Propagate->GetUVW(2));  // ECEF speed in body axis
+    net->v_body_w = static_cast<float>(Propagate->GetUVW(3));  // ECEF speed in body axis
+
+    // Accelerations
+    net->A_X_pilot = static_cast<float>(Auxiliary->GetPilotAccel(1));  // X body accel, ft/s/s
+    net->A_Y_pilot = static_cast<float>(Auxiliary->GetPilotAccel(2));  // Y body accel, ft/s/s
+    net->A_Z_pilot = static_cast<float>(Auxiliary->GetPilotAccel(3));  // Z body accel, ft/s/s
+
+    // Stall
+    net->stall_warning = 0.0;  // 0.0 - 1.0 indicating the amount of stall
+    net->slip_deg = static_cast<float>(Auxiliary->Getbeta(inDegrees));  // slip ball deflection, deg
+
+    // Engine status
+    if (Propulsion->GetNumEngines() > FGNetFDM::FG_MAX_ENGINES && FDMExec->GetSimTime() == 0.0)
+        cerr << "This vehicle has " << Propulsion->GetNumEngines() << " engines, but the current "
+             << endl
+             << "version of FlightGear's FGNetFDM only supports " << FGNetFDM::FG_MAX_ENGINES
+             << " engines." << endl
+             << "Only the first " << FGNetFDM::FG_MAX_ENGINES << " engines will be used." << endl;
+
+    net->num_engines = std::min(static_cast<unsigned int>(FGNetFDM::FG_MAX_ENGINES),
+                                Propulsion->GetNumEngines());  // Number of valid engines
+
+    for (i = 0; i < net->num_engines; i++) {
+        if (Propulsion->GetEngine(i)->GetRunning())
+            net->eng_state[i] = 2;  // Engine state running
+        else if (Propulsion->GetEngine(i)->GetCranking())
+            net->eng_state[i] = 1;  // Engine state cranking
+        else
+            net->eng_state[i] = 0;  // Engine state off
+
+        switch (Propulsion->GetEngine(i)->GetType()) {
+            case (FGEngine::etRocket):
+                break;
+            case (FGEngine::etPiston):
+                net->rpm[i] = static_cast<float>(
+                    (static_cast<FGPiston*>(Propulsion->GetEngine(i))->getRPM()));
+                net->fuel_flow[i] = static_cast<float>(
+                    ((static_cast<FGPiston*>(Propulsion->GetEngine(i))->getFuelFlow_gph())));
+                net->fuel_px[i] = 0;  // Fuel pressure, psi  (N/A in current model)
+                net->egt[i] = static_cast<float>(
+                    ((static_cast<FGPiston*>(Propulsion->GetEngine(i))->GetEGT())));
+                net->cht[i] = static_cast<float>(((
+                    static_cast<FGPiston*>(Propulsion->GetEngine(i))->getCylinderHeadTemp_degF())));
+                net->mp_osi[i] = static_cast<float>(((
+                    static_cast<FGPiston*>(Propulsion->GetEngine(i))->getManifoldPressure_inHg())));
+                net->oil_temp[i] = static_cast<float>(
+                    ((static_cast<FGPiston*>(Propulsion->GetEngine(i))->getOilTemp_degF())));
+                net->oil_px[i] = static_cast<float>(
+                    ((static_cast<FGPiston*>(Propulsion->GetEngine(i))->getOilPressure_psi())));
+                net->tit[i] = 0;  // Turbine Inlet Temperature  (N/A for piston)
+                break;
+            case (FGEngine::etTurbine):
+                break;
+            case (FGEngine::etTurboprop):
+                break;
+            case (FGEngine::etElectric):
+                break;
+            case (FGEngine::etUnknown):
+                break;
+        }
     }
-  }
-
-  // Consumables
-  if (Propulsion->GetNumTanks() > FGNetFDM::FG_MAX_TANKS && FDMExec->GetSimTime() == 0.0)
-    cerr << "This vehicle has " << Propulsion->GetNumTanks() << " tanks, but the current " << endl
-         << "version of FlightGear's FGNetFDM only supports " << FGNetFDM::FG_MAX_TANKS << " tanks." << endl
-         << "Only the first " << FGNetFDM::FG_MAX_TANKS << " tanks will be used." << endl;
-
-  net->num_tanks = std::min(static_cast<unsigned int>(FGNetFDM::FG_MAX_TANKS),
-                            Propulsion->GetNumTanks());   // Max number of fuel tanks
-
-  for (i=0; i < net->num_tanks; i++) {
-      net->fuel_quantity[i] = static_cast<float>((static_cast<FGTank *>(Propulsion->GetTank(i))->GetContents()));
-  }
-
-  // Gear status
-  if (GroundReactions->GetNumGearUnits() > FGNetFDM::FG_MAX_WHEELS && FDMExec->GetSimTime() == 0.0)
-    cerr << "This vehicle has " << GroundReactions->GetNumGearUnits() << " bogeys, but the current " << endl
-         << "version of FlightGear's FGNetFDM only supports " << FGNetFDM::FG_MAX_WHEELS << " bogeys." << endl
-         << "Only the first " << FGNetFDM::FG_MAX_WHEELS << " bogeys will be used." << endl;
-
-  net->num_wheels  = std::min(static_cast<int>(FGNetFDM::FG_MAX_WHEELS),
-                              GroundReactions->GetNumGearUnits());
-
-  for (i=0; i < net->num_wheels; i++) {
-    net->wow[i]              = GroundReactions->GetGearUnit(i)->GetWOW();
-    if (GroundReactions->GetGearUnit(i)->GetGearUnitDown())
-      net->gear_pos[i]      = 1;  // gear down, using FCS convention
-    else
-      net->gear_pos[i]      = 0;  // gear up, using FCS convention
-    net->gear_steer[i]       = static_cast<float>(GroundReactions->GetGearUnit(i)->GetSteerNorm());
-    net->gear_compression[i] = static_cast<float>(GroundReactions->GetGearUnit(i)->GetCompLen());
-
-    net->gear_pos[i] = 0; // MOD: Force gear up
-  }
-
-  // Environment
-  net->cur_time    = static_cast<uint64_t>(1234567890);    // Friday, Feb 13, 2009, 23:31:30 UTC (not processed by FGFS anyway)
-  net->warp        = 0;                       // offset in seconds to unix time
-  net->visibility  = 25000.0;                 // visibility in meters (for env. effects)
-
-  // Control surface positions (normalized values)
-  net->elevator          = static_cast<float>(FCS->GetDePos(ofNorm));    // Norm Elevator Pos, --
-  net->elevator_trim_tab = static_cast<float>(FCS->GetPitchTrimCmd());   // Norm Elev Trim Tab Pos, --
-  net->left_flap         = static_cast<float>(FCS->GetDfPos(ofNorm));    // Norm Flap Pos, --
-  net->right_flap        = static_cast<float>(FCS->GetDfPos(ofNorm));    // Norm Flap Pos, --
-  net->left_aileron      = static_cast<float>(FCS->GetDaLPos(ofNorm));   // Norm L Aileron Pos, --
-  net->right_aileron     = static_cast<float>(FCS->GetDaRPos(ofNorm));   // Norm R Aileron Pos, --
-  net->rudder            = static_cast<float>(FCS->GetDrPos(ofNorm));    // Norm Rudder Pos, --
-  net->nose_wheel        = static_cast<float>(FCS->GetDrPos(ofNorm));    // *** FIX ***  Using Rudder Pos for NWS, --
-  net->speedbrake        = static_cast<float>(FCS->GetDsbPos(ofNorm));   // Norm Speedbrake Pos, --
-  net->spoilers          = static_cast<float>(FCS->GetDspPos(ofNorm));   // Norm Spoiler Pos, --
-
-  // Convert the net buffer to network format
-  if ( isLittleEndian ) {
-    net->version = htonl(net->version);
-
-    htond(net->longitude);
-    htond(net->latitude);
-    htond(net->altitude);
-    htonf(net->agl);
-    htonf(net->phi);
-    htonf(net->theta);
-    htonf(net->psi);
-    htonf(net->alpha);
-    htonf(net->beta);
-
-    htonf(net->phidot);
-    htonf(net->thetadot);
-    htonf(net->psidot);
-    htonf(net->vcas);
-    htonf(net->climb_rate);
-    htonf(net->v_north);
-    htonf(net->v_east);
-    htonf(net->v_down);
-    htonf(net->v_body_u);
-    htonf(net->v_body_v);
-    htonf(net->v_body_w);
-
-    htonf(net->A_X_pilot);
-    htonf(net->A_Y_pilot);
-    htonf(net->A_Z_pilot);
-
-    htonf(net->stall_warning);
-    htonf(net->slip_deg);
-
-    for (i=0; i < net->num_engines; ++i) {
-      net->eng_state[i] = htonl(net->eng_state[i]);
-      htonf(net->rpm[i]);
-      htonf(net->fuel_flow[i]);
-      htonf(net->fuel_px[i]);
-      htonf(net->egt[i]);
-      htonf(net->cht[i]);
-      htonf(net->mp_osi[i]);
-      htonf(net->tit[i]);
-      htonf(net->oil_temp[i]);
-      htonf(net->oil_px[i]);
+
+    // Consumables
+    if (Propulsion->GetNumTanks() > FGNetFDM::FG_MAX_TANKS && FDMExec->GetSimTime() == 0.0)
+        cerr << "This vehicle has " << Propulsion->GetNumTanks() << " tanks, but the current "
+             << endl
+             << "version of FlightGear's FGNetFDM only supports " << FGNetFDM::FG_MAX_TANKS
+             << " tanks." << endl
+             << "Only the first " << FGNetFDM::FG_MAX_TANKS << " tanks will be used." << endl;
+
+    net->num_tanks = std::min(static_cast<unsigned int>(FGNetFDM::FG_MAX_TANKS),
+                              Propulsion->GetNumTanks());  // Max number of fuel tanks
+
+    for (i = 0; i < net->num_tanks; i++) {
+        net->fuel_quantity[i] =
+            static_cast<float>((static_cast<FGTank*>(Propulsion->GetTank(i))->GetContents()));
     }
-    net->num_engines = htonl(net->num_engines);
 
-    for (i=0; i < net->num_tanks; ++i) {
-      htonf(net->fuel_quantity[i]);
+    // Gear status
+    if (GroundReactions->GetNumGearUnits() > FGNetFDM::FG_MAX_WHEELS
+        && FDMExec->GetSimTime() == 0.0)
+        cerr << "This vehicle has " << GroundReactions->GetNumGearUnits()
+             << " bogeys, but the current " << endl
+             << "version of FlightGear's FGNetFDM only supports " << FGNetFDM::FG_MAX_WHEELS
+             << " bogeys." << endl
+             << "Only the first " << FGNetFDM::FG_MAX_WHEELS << " bogeys will be used." << endl;
+
+    net->num_wheels =
+        std::min(static_cast<int>(FGNetFDM::FG_MAX_WHEELS), GroundReactions->GetNumGearUnits());
+
+    for (i = 0; i < net->num_wheels; i++) {
+        net->wow[i] = GroundReactions->GetGearUnit(i)->GetWOW();
+        if (GroundReactions->GetGearUnit(i)->GetGearUnitDown())
+            net->gear_pos[i] = 1;  // gear down, using FCS convention
+        else
+            net->gear_pos[i] = 0;  // gear up, using FCS convention
+        net->gear_steer[i] = static_cast<float>(GroundReactions->GetGearUnit(i)->GetSteerNorm());
+        net->gear_compression[i] =
+            static_cast<float>(GroundReactions->GetGearUnit(i)->GetCompLen());
+
+        net->gear_pos[i] = 0;  // MOD: Force gear up
     }
-    net->num_tanks = htonl(net->num_tanks);
 
-    for (i=0; i < net->num_wheels; ++i) {
-      net->wow[i] = htonl(net->wow[i]);
-      htonf(net->gear_pos[i]);
-      htonf(net->gear_steer[i]);
-      htonf(net->gear_compression[i]);
+    // Environment
+    net->cur_time = static_cast<uint64_t>(
+        1234567890);            // Friday, Feb 13, 2009, 23:31:30 UTC (not processed by FGFS anyway)
+    net->warp = 0;              // offset in seconds to unix time
+    net->visibility = 25000.0;  // visibility in meters (for env. effects)
+
+    // Control surface positions (normalized values)
+    net->elevator = static_cast<float>(FCS->GetDePos(ofNorm));  // Norm Elevator Pos, --
+    net->elevator_trim_tab =
+        static_cast<float>(FCS->GetPitchTrimCmd());                   // Norm Elev Trim Tab Pos, --
+    net->left_flap = static_cast<float>(FCS->GetDfPos(ofNorm));       // Norm Flap Pos, --
+    net->right_flap = static_cast<float>(FCS->GetDfPos(ofNorm));      // Norm Flap Pos, --
+    net->left_aileron = static_cast<float>(FCS->GetDaLPos(ofNorm));   // Norm L Aileron Pos, --
+    net->right_aileron = static_cast<float>(FCS->GetDaRPos(ofNorm));  // Norm R Aileron Pos, --
+    net->rudder = static_cast<float>(FCS->GetDrPos(ofNorm));          // Norm Rudder Pos, --
+    net->nose_wheel =
+        static_cast<float>(FCS->GetDrPos(ofNorm));  // *** FIX ***  Using Rudder Pos for NWS, --
+    net->speedbrake = static_cast<float>(FCS->GetDsbPos(ofNorm));  // Norm Speedbrake Pos, --
+    net->spoilers = static_cast<float>(FCS->GetDspPos(ofNorm));    // Norm Spoiler Pos, --
+
+    // Convert the net buffer to network format
+    if (isLittleEndian) {
+        net->version = htonl(net->version);
+
+        htond(net->longitude);
+        htond(net->latitude);
+        htond(net->altitude);
+        htonf(net->agl);
+        htonf(net->phi);
+        htonf(net->theta);
+        htonf(net->psi);
+        htonf(net->alpha);
+        htonf(net->beta);
+
+        htonf(net->phidot);
+        htonf(net->thetadot);
+        htonf(net->psidot);
+        htonf(net->vcas);
+        htonf(net->climb_rate);
+        htonf(net->v_north);
+        htonf(net->v_east);
+        htonf(net->v_down);
+        htonf(net->v_body_u);
+        htonf(net->v_body_v);
+        htonf(net->v_body_w);
+
+        htonf(net->A_X_pilot);
+        htonf(net->A_Y_pilot);
+        htonf(net->A_Z_pilot);
+
+        htonf(net->stall_warning);
+        htonf(net->slip_deg);
+
+        for (i = 0; i < net->num_engines; ++i) {
+            net->eng_state[i] = htonl(net->eng_state[i]);
+            htonf(net->rpm[i]);
+            htonf(net->fuel_flow[i]);
+            htonf(net->fuel_px[i]);
+            htonf(net->egt[i]);
+            htonf(net->cht[i]);
+            htonf(net->mp_osi[i]);
+            htonf(net->tit[i]);
+            htonf(net->oil_temp[i]);
+            htonf(net->oil_px[i]);
+        }
+        net->num_engines = htonl(net->num_engines);
+
+        for (i = 0; i < net->num_tanks; ++i) {
+            htonf(net->fuel_quantity[i]);
+        }
+        net->num_tanks = htonl(net->num_tanks);
+
+        for (i = 0; i < net->num_wheels; ++i) {
+            net->wow[i] = htonl(net->wow[i]);
+            htonf(net->gear_pos[i]);
+            htonf(net->gear_steer[i]);
+            htonf(net->gear_compression[i]);
+        }
+        net->num_wheels = htonl(net->num_wheels);
+
+        net->cur_time = htonl(net->cur_time);
+        net->warp = htonl(net->warp);
+        htonf(net->visibility);
+
+        htonf(net->elevator);
+        htonf(net->elevator_trim_tab);
+        htonf(net->left_flap);
+        htonf(net->right_flap);
+        htonf(net->left_aileron);
+        htonf(net->right_aileron);
+        htonf(net->rudder);
+        htonf(net->nose_wheel);
+        htonf(net->speedbrake);
+        htonf(net->spoilers);
     }
-    net->num_wheels = htonl(net->num_wheels);
-
-    net->cur_time = htonl(net->cur_time);
-    net->warp = htonl(net->warp);
-    htonf(net->visibility);
-
-    htonf(net->elevator);
-    htonf(net->elevator_trim_tab);
-    htonf(net->left_flap);
-    htonf(net->right_flap);
-    htonf(net->left_aileron);
-    htonf(net->right_aileron);
-    htonf(net->rudder);
-    htonf(net->nose_wheel);
-    htonf(net->speedbrake);
-    htonf(net->spoilers);
-  }
 }
-} // namespace JSBSim
+}  // namespace JSBSim
diff --git a/src/plugins/motion/JSBSimModel/JSBSimModel.cpp b/src/plugins/motion/JSBSimModel/JSBSimModel.cpp
index 2b54581d99..6492242010 100644
--- a/src/plugins/motion/JSBSimModel/JSBSimModel.cpp
+++ b/src/plugins/motion/JSBSimModel/JSBSimModel.cpp
@@ -30,17 +30,16 @@
  *
  */
 
-#include <scrimmage/plugins/motion/JSBSimModel/JSBSimModel.h>
-#include <scrimmage/plugins/motion/JSBSimModel/FGOutputFGMod.h>
-
-#include <scrimmage/common/VariableIO.h>
 #include <scrimmage/common/Utilities.h>
-#include <scrimmage/math/State.h>
+#include <scrimmage/common/VariableIO.h>
 #include <scrimmage/entity/Entity.h>
-#include <scrimmage/parse/ParseUtils.h>
+#include <scrimmage/math/Angles.h>
+#include <scrimmage/math/State.h>
 #include <scrimmage/parse/MissionParse.h>
+#include <scrimmage/parse/ParseUtils.h>
 #include <scrimmage/plugin_manager/RegisterPlugin.h>
-#include <scrimmage/math/Angles.h>
+#include <scrimmage/plugins/motion/JSBSimModel/FGOutputFGMod.h>
+#include <scrimmage/plugins/motion/JSBSimModel/JSBSimModel.h>
 
 // c system
 #include <JSBSim/initialization/FGTrim.h>
@@ -49,8 +48,8 @@
 // <none>
 
 // other
-#include <JSBSim/simgear/misc/sg_path.hxx>
 #include <GeographicLib/LocalCartesian.hpp>
+#include <JSBSim/simgear/misc/sg_path.hxx>
 
 #define meters2feet 3.28084
 #define feet2meters (1.0 / meters2feet)
@@ -69,15 +68,14 @@ std::tuple<int, int, int> JSBSimModel::version() {
     return std::tuple<int, int, int>(0, 0, 1);
 }
 
-bool JSBSimModel::init(std::map<std::string, std::string> &info,
-                       std::map<std::string, std::string> &params) {
+bool JSBSimModel::init(std::map<std::string, std::string>& info,
+                       std::map<std::string, std::string>& params) {
     angles_from_jsbsim_ = Angles(0, Angles::Type::GPS, Angles::Type::EUCLIDEAN);
     angles_to_jsbsim_ = Angles(0, Angles::Type::EUCLIDEAN, Angles::Type::GPS);
 
     use_pitch_ = str2bool(params.at("use_pitch"));
-    std::string z_name =  use_pitch_ ?
-        vars_.type_map().at(VariableIO::Type::desired_pitch) :
-        vars_.type_map().at(VariableIO::Type::desired_altitude);
+    std::string z_name = use_pitch_ ? vars_.type_map().at(VariableIO::Type::desired_pitch)
+                                    : vars_.type_map().at(VariableIO::Type::desired_altitude);
 
     speed_idx_ = vars_.declare(VariableIO::Type::desired_speed, VariableIO::Direction::In);
     roll_idx_ = vars_.declare(VariableIO::Type::desired_roll, VariableIO::Direction::In);
@@ -93,7 +91,7 @@ bool JSBSimModel::init(std::map<std::string, std::string> &info,
         std::string ip = get<std::string>("flightgear_ip", params, "localhost");
         std::string port = get<std::string>("flightgear_port", params, "5600");
         std::string protocol = get<std::string>("flightgear_protocol", params, "UDP");
-        std::string name = ip + ":" + protocol + "/" + port; // localhost:UDP/5600
+        std::string name = ip + ":" + protocol + "/" + port;  // localhost:UDP/5600
 
         output_fg_->SetIdx(0);
         output_fg_->SetOutputName(name);
@@ -114,9 +112,9 @@ bool JSBSimModel::init(std::map<std::string, std::string> &info,
     exec_->SetEnginePath(SGPath("engine"));
     exec_->SetSystemsPath(SGPath("systems"));
 
-    exec_->LoadScript(SGPath("scripts/"+info["script_name"]));
+    exec_->LoadScript(SGPath("scripts/" + info["script_name"]));
 
-    JSBSim::FGInitialCondition *ic = exec_->GetIC();
+    JSBSim::FGInitialCondition* ic = exec_->GetIC();
     if (info.count("latitude") > 0) {
         ic->SetLatitudeDegIC(std::stod(info["latitude"]));
     }
@@ -192,23 +190,21 @@ bool JSBSimModel::init(std::map<std::string, std::string> &info,
 
     // Save state
     parent_->projection()->Forward(latitude_node_->getDoubleValue(),
-                                  longitude_node_->getDoubleValue(),
-                                  altitude_node_->getDoubleValue() * feet2meters,
-                                  state_->pos()(0), state_->pos()(1), state_->pos()(2));
+                                   longitude_node_->getDoubleValue(),
+                                   altitude_node_->getDoubleValue() * feet2meters, state_->pos()(0),
+                                   state_->pos()(1), state_->pos()(2));
 
     angles_from_jsbsim_.set_angle(ang::rad2deg(yaw_node_->getDoubleValue()));
 
-    state_->quat().set(roll_node_->getDoubleValue(),
-                      -pitch_node_->getDoubleValue(),
-                      ang::deg2rad(angles_from_jsbsim_.angle()));
+    state_->quat().set(roll_node_->getDoubleValue(), -pitch_node_->getDoubleValue(),
+                       ang::deg2rad(angles_from_jsbsim_.angle()));
 
     state_->vel() << vel_east_node_->getDoubleValue() * feet2meters,
-                    vel_north_node_->getDoubleValue() * feet2meters,
-                    -vel_down_node_->getDoubleValue() * feet2meters;
+        vel_north_node_->getDoubleValue() * feet2meters,
+        -vel_down_node_->getDoubleValue() * feet2meters;
 
-    state_->ang_vel() << p_node_->getDoubleValue(),
-                         q_node_->getDoubleValue(),
-                         p_node_->getDoubleValue();
+    state_->ang_vel() << p_node_->getDoubleValue(), q_node_->getDoubleValue(),
+        p_node_->getDoubleValue();
 
     return true;
 }
@@ -233,8 +229,8 @@ bool JSBSimModel::step(double time, double dt) {
         // Set desired altitude (we just need the desired altitude, use the current
         // x,y as placeholders).
         double lat_curr, lon_curr, alt_result;
-        parent_->projection()->Reverse(state_->pos()(0), state_->pos()(1), desired_alt,
-                                       lat_curr, lon_curr, alt_result);
+        parent_->projection()->Reverse(state_->pos()(0), state_->pos()(1), desired_alt, lat_curr,
+                                       lon_curr, alt_result);
 
         desired_altitude_node_->setDoubleValue(alt_result * meters2feet);
     }
@@ -252,31 +248,28 @@ bool JSBSimModel::step(double time, double dt) {
 
     parent_->projection()->Forward(latitude_node_->getDoubleValue(),
                                    longitude_node_->getDoubleValue(),
-                                   altitude_node_->getDoubleValue() * feet2meters,
-                                   state_->pos()(0), state_->pos()(1), state_->pos()(2));
+                                   altitude_node_->getDoubleValue() * feet2meters, state_->pos()(0),
+                                   state_->pos()(1), state_->pos()(2));
 
     angles_from_jsbsim_.set_angle(ang::rad2deg(yaw_node_->getDoubleValue()));
 
-    state_->quat().set(roll_node_->getDoubleValue(),
-                      -pitch_node_->getDoubleValue(),
-                      ang::deg2rad(angles_from_jsbsim_.angle()));
+    state_->quat().set(roll_node_->getDoubleValue(), -pitch_node_->getDoubleValue(),
+                       ang::deg2rad(angles_from_jsbsim_.angle()));
 
     state_->vel() << vel_east_node_->getDoubleValue() * feet2meters,
-                    vel_north_node_->getDoubleValue() * feet2meters,
-                    -vel_down_node_->getDoubleValue() * feet2meters;
+        vel_north_node_->getDoubleValue() * feet2meters,
+        -vel_down_node_->getDoubleValue() * feet2meters;
 
     // save what was used as the input
     return true;
 }
 
-void JSBSimModel::teleport(StatePtr &state) {
+void JSBSimModel::teleport(StatePtr& state) {
     double lat, lon, alt;
-    parent_->projection()->Reverse(state->pos()(0),
-                                   state->pos()(1),
-                                   state->pos()(2),
-                                   lat, lon, alt);
+    parent_->projection()->Reverse(state->pos()(0), state->pos()(1), state->pos()(2), lat, lon,
+                                   alt);
 
-    JSBSim::FGInitialCondition *ic = exec_->GetIC();
+    JSBSim::FGInitialCondition* ic = exec_->GetIC();
     ic->SetLatitudeDegIC(lat);
     ic->SetLongitudeDegIC(lon);
     ic->SetAltitudeASLFtIC(alt * meters2feet);
@@ -288,5 +281,5 @@ void JSBSimModel::teleport(StatePtr &state) {
     exec_->Setdt(dt_);
     exec_->Run();
 }
-} // namespace motion
-} // namespace scrimmage
+}  // namespace motion
+}  // namespace scrimmage
diff --git a/src/plugins/motion/Multirotor/Multirotor.cpp b/src/plugins/motion/Multirotor/Multirotor.cpp
index 8e21ffb73b..f50602cbf8 100644
--- a/src/plugins/motion/Multirotor/Multirotor.cpp
+++ b/src/plugins/motion/Multirotor/Multirotor.cpp
@@ -30,30 +30,25 @@
  *
  */
 
-#include <scrimmage/plugins/motion/Multirotor/Multirotor.h>
-
 #include <scrimmage/common/Utilities.h>
-#include <scrimmage/parse/ParseUtils.h>
-#include <scrimmage/math/Angles.h>
-#include <scrimmage/plugin_manager/RegisterPlugin.h>
 #include <scrimmage/entity/Entity.h>
+#include <scrimmage/math/Angles.h>
 #include <scrimmage/parse/MissionParse.h>
-
-#include <scrimmage/proto/Shape.pb.h>
+#include <scrimmage/parse/ParseUtils.h>
+#include <scrimmage/plugin_manager/RegisterPlugin.h>
+#include <scrimmage/plugins/motion/Multirotor/Multirotor.h>
 #include <scrimmage/proto/ProtoConversions.h>
-
-#include <iostream>
+#include <scrimmage/proto/Shape.pb.h>
 
 #include <boost/algorithm/clamp.hpp>
+#include <iostream>
 
 namespace sc = scrimmage;
 
 using std::cout;
 using std::endl;
 
-REGISTER_PLUGIN(scrimmage::MotionModel,
-                scrimmage::motion::Multirotor,
-                Multirotor_plugin)
+REGISTER_PLUGIN(scrimmage::MotionModel, scrimmage::motion::Multirotor, Multirotor_plugin)
 
 namespace scrimmage {
 namespace motion {
@@ -62,13 +57,12 @@ Multirotor::Multirotor() : write_csv_(false) {
     x_.resize(MODEL_NUM_ITEMS);
 }
 
-bool Multirotor::init(std::map<std::string, std::string> &info,
-                      std::map<std::string, std::string> &params) {
-
+bool Multirotor::init(std::map<std::string, std::string>& info,
+                      std::map<std::string, std::string>& params) {
     show_shapes_ = sc::get<bool>("show_shapes", params, false);
 
     x_.resize(MODEL_NUM_ITEMS);
-    Eigen::Vector3d &pos = state_->pos();
+    Eigen::Vector3d& pos = state_->pos();
 
     x_[U] = 0;
     x_[V] = 0;
@@ -98,8 +92,7 @@ bool Multirotor::init(std::map<std::string, std::string> &info,
 
     // Parse inertia matrix
     std::vector<std::vector<std::string>> vecs;
-    std::string inertia_matrix = sc::get<std::string>("inertia_matrix",
-                                                      params, "");
+    std::string inertia_matrix = sc::get<std::string>("inertia_matrix", params, "");
     bool valid_inertia = false;
     if (!sc::get_vec_of_vecs(inertia_matrix, vecs)) {
         cout << "Failed to parse inertia_matrix:" << inertia_matrix << endl;
@@ -133,8 +126,7 @@ bool Multirotor::init(std::map<std::string, std::string> &info,
 
     // Parse rotor configuration
     std::vector<std::vector<std::string>> vecs_r;
-    std::string rotor_config = sc::get<std::string>("rotor_config",
-                                                    params, "");
+    std::string rotor_config = sc::get<std::string>("rotor_config", params, "");
     if (!sc::get_vec_of_vecs(rotor_config, vecs_r)) {
         cout << "Failed to parse rotor_config:" << rotor_config << endl;
         return false;
@@ -156,9 +148,7 @@ bool Multirotor::init(std::map<std::string, std::string> &info,
             r.set_direction(Rotor::Direction::CW);
         }
 
-        r.set_offset(Eigen::Vector3d(std::stod(vec[1]),
-                                     std::stod(vec[2]),
-                                     std::stod(vec[3])));
+        r.set_offset(Eigen::Vector3d(std::stod(vec[1]), std::stod(vec[2]), std::stod(vec[3])));
 
         r.quat() = sc::Quaternion(sc::Angles::deg2rad(std::stod(vec[4])),
                                   sc::Angles::deg2rad(std::stod(vec[5])),
@@ -166,29 +156,22 @@ bool Multirotor::init(std::map<std::string, std::string> &info,
         rotors_.push_back(r);
     }
 
-
     motor_idx_vec_.resize(rotors_.size());
     ctrl_u_.resize(rotors_.size());
     for (unsigned int i = 0; i < rotors_.size(); i++) {
-        std::string name = "motor_"+std::to_string(i);
+        std::string name = "motor_" + std::to_string(i);
         motor_idx_vec_(i) = vars_.declare(name, VariableIO::Direction::In);
     }
 
-
     write_csv_ = sc::get<bool>("write_csv", params, false);
     if (write_csv_) {
-        csv_.open_output(parent_->mp()->log_dir() + "/"
-                         + std::to_string(parent_->id().id())
+        csv_.open_output(parent_->mp()->log_dir() + "/" + std::to_string(parent_->id().id())
                          + "-states.csv");
 
-        csv_.set_column_headers(sc::CSV::Headers{"t",
-                    "x", "y", "z",
-                    "U", "V", "W",
-                    "P", "Q", "R",
-                    "AXb", "AYb", "AZb",
-                    "WXDOTb", "WYDOTb", "WZDOTb",
-                    "roll", "pitch", "yaw",
-                    "w_1", "w_2", "w_3", "w_4"});
+        csv_.set_column_headers(
+            sc::CSV::Headers{"t",    "x",     "y",   "z",   "U",   "V",      "W",      "P",
+                             "Q",    "R",     "AXb", "AYb", "AZb", "WXDOTb", "WYDOTb", "WZDOTb",
+                             "roll", "pitch", "yaw", "w_1", "w_2", "w_3",    "w_4"});
     }
 
     return true;
@@ -221,10 +204,9 @@ bool Multirotor::step(double time, double dt) {
 
     // Apply any external forces
     force_ext_body_ = state_->quat().rotate_reverse(ext_force_);
-    ext_force_ = Eigen::Vector3d::Zero(); // reset ext_force_ member variable
-
-    ode_step(dt); // step the motion model ODE solver
+    ext_force_ = Eigen::Vector3d::Zero();  // reset ext_force_ member variable
 
+    ode_step(dt);  // step the motion model ODE solver
 
     state_->quat().set(x_[q0], x_[q1], x_[q2], x_[q3]);
     state_->quat().normalize();
@@ -236,7 +218,6 @@ bool Multirotor::step(double time, double dt) {
     Eigen::Vector3d angular_vel(x_[P], x_[Q], x_[R]);
     Eigen::Vector3d angular_acc = (angular_vel - prev_angular_vel) / dt;
 
-
     // Convert local coordinates to world coordinates
     state_->pos() << x_[Xw], x_[Yw], x_[Zw];
     state_->vel() << x_[Uw], x_[Vw], x_[Ww];
@@ -260,42 +241,41 @@ bool Multirotor::step(double time, double dt) {
         line->set_opacity(1.0);
         sc::set(line->mutable_color(), 0, 255, 255);
         sc::set(line->mutable_line()->mutable_start(), state_->pos());
-        sc::set(line->mutable_line()->mutable_end(), state_->pos() + state_->ang_vel()*10);
+        sc::set(line->mutable_line()->mutable_end(), state_->pos() + state_->ang_vel() * 10);
         draw_shape(line);
     }
 
     if (write_csv_) {
         // Log state to CSV
-        csv_.append(sc::CSV::Pairs{
-                {"t", time},
-                {"x", x_[Xw]},
-                {"y", x_[Yw]},
-                {"z", x_[Zw]},
-                {"U", x_[U]},
-                {"V", x_[V]},
-                {"W", x_[W]},
-                {"P", x_[P]},
-                {"Q", x_[Q]},
-                {"R", x_[R]},
-                {"AXb", linear_accel_body_(0)},
-                {"AYb", linear_accel_body_(1)},
-                {"AZb", linear_accel_body_(2)},
-                {"WXDOTb", ang_accel_body_(0)},
-                {"WYDOTb", ang_accel_body_(1)},
-                {"WZDOTb", ang_accel_body_(2)},
-                {"roll", state_->quat().roll()},
-                {"pitch", state_->quat().pitch()},
-                {"yaw", state_->quat().yaw()},
-                {"w_1", ctrl_u_(0)},
-                {"w_2", ctrl_u_(1)},
-                {"w_3", ctrl_u_(2)},
-                {"w_4", ctrl_u_(3)}});
+        csv_.append(sc::CSV::Pairs{{"t", time},
+                                   {"x", x_[Xw]},
+                                   {"y", x_[Yw]},
+                                   {"z", x_[Zw]},
+                                   {"U", x_[U]},
+                                   {"V", x_[V]},
+                                   {"W", x_[W]},
+                                   {"P", x_[P]},
+                                   {"Q", x_[Q]},
+                                   {"R", x_[R]},
+                                   {"AXb", linear_accel_body_(0)},
+                                   {"AYb", linear_accel_body_(1)},
+                                   {"AZb", linear_accel_body_(2)},
+                                   {"WXDOTb", ang_accel_body_(0)},
+                                   {"WYDOTb", ang_accel_body_(1)},
+                                   {"WZDOTb", ang_accel_body_(2)},
+                                   {"roll", state_->quat().roll()},
+                                   {"pitch", state_->quat().pitch()},
+                                   {"yaw", state_->quat().yaw()},
+                                   {"w_1", ctrl_u_(0)},
+                                   {"w_2", ctrl_u_(1)},
+                                   {"w_3", ctrl_u_(2)},
+                                   {"w_4", ctrl_u_(3)}});
     }
 
     return true;
 }
 
-void Multirotor::model(const vector_t &x , vector_t &dxdt , double t) {
+void Multirotor::model(const vector_t& x, vector_t& dxdt, double t) {
     // Omega values for each rotor
     Eigen::VectorXd omega = ctrl_u_;
     Eigen::VectorXd omega_sq = omega.cwiseProduct(omega);
@@ -306,8 +286,9 @@ void Multirotor::model(const vector_t &x , vector_t &dxdt , double t) {
     Eigen::VectorXd rotor_torque(rotors_.size());
 
     for (unsigned int i = 0; i < rotors_.size(); i++) {
-        rotor_thrust(i) =  c_T_ * omega_sq(i);
-        if (std::abs(omega[i] - wmin_) < std::numeric_limits<double>::epsilon()) { // if minimum throttle, servo is off
+        rotor_thrust(i) = c_T_ * omega_sq(i);
+        if (std::abs(omega[i] - wmin_)
+            < std::numeric_limits<double>::epsilon()) {  // if minimum throttle, servo is off
             rotor_thrust[i] = 0.0;
         }
         rotor_torque(i) = rotors_[i].direction() * c_Q_ * omega_sq(i);
@@ -315,7 +296,7 @@ void Multirotor::model(const vector_t &x , vector_t &dxdt , double t) {
     }
 
     // Calculate force from weight in body frame:
-    Eigen::Vector3d gravity_vector(0, 0, -mass_*g_);
+    Eigen::Vector3d gravity_vector(0, 0, -mass_ * g_);
     Eigen::Vector3d F_weight = state_->quat().rotate_reverse(gravity_vector);
 
     // Calculate force from drag (TODO: Check source)
@@ -324,24 +305,25 @@ void Multirotor::model(const vector_t &x , vector_t &dxdt , double t) {
     Eigen::Vector3d F_drag = vel_body * (-0.5 * c_D_ * vel_mag);
 
     // Calculate normal force
-    force_ext_body_[2] = force_ext_body_[2]-F_thrust[2];
-    if (force_ext_body_[2] > 0) { // only true if contacting ground
-        force_ext_body_[2] = -1.0*(F_weight[2]) - F_thrust[2]; // correctly calculate normal force
+    force_ext_body_[2] = force_ext_body_[2] - F_thrust[2];
+    if (force_ext_body_[2] > 0) {  // only true if contacting ground
+        force_ext_body_[2] =
+            -1.0 * (F_weight[2]) - F_thrust[2];  // correctly calculate normal force
     }
 
-    if (force_ext_body_[2] < 0) { // normal force only pushes up from the ground
+    if (force_ext_body_[2] < 0) {  // normal force only pushes up from the ground
         force_ext_body_[2] = 0.0;
     }
 
     // Calculate total force
     Eigen::Vector3d F_total = F_thrust + F_weight + F_drag + force_ext_body_;
 
-    dxdt[U] = x[V]*x[R] - x[W]*x[Q] + F_total(0) / mass_;
-    dxdt[V] = x[W]*x[P] - x[U]*x[R] + F_total(1) / mass_;
-    dxdt[W] = x[U]*x[Q] - x[V]*x[P] + F_total(2) / mass_;
+    dxdt[U] = x[V] * x[R] - x[W] * x[Q] + F_total(0) / mass_;
+    dxdt[V] = x[W] * x[P] - x[U] * x[R] + F_total(1) / mass_;
+    dxdt[W] = x[U] * x[Q] - x[V] * x[P] + F_total(2) / mass_;
 
     // Calculate moments from thrust
-    Eigen::Vector3d Moments_thrust(0, 0, 0); // L, M, N
+    Eigen::Vector3d Moments_thrust(0, 0, 0);  // L, M, N
 
     for (unsigned int i = 0; i < rotors_.size(); i++) {
         double length = rotors_[i].offset_length();
@@ -355,17 +337,17 @@ void Multirotor::model(const vector_t &x , vector_t &dxdt , double t) {
     Eigen::Vector3d Moments_total = Moments_thrust;
 
     Eigen::Vector3d pqr(x_[P], x_[Q], x_[R]);
-    Eigen::Vector3d pqr_dot = I_inv_ * (Moments_total - pqr.cross(I_*pqr));
+    Eigen::Vector3d pqr_dot = I_inv_ * (Moments_total - pqr.cross(I_ * pqr));
     dxdt[P] = pqr_dot(0);
     dxdt[Q] = pqr_dot(1);
     dxdt[R] = pqr_dot(2);
 
     // Update quaternion from angular accelerations
     double lambda = 1 - (pow(x[q0], 2) + pow(x[q1], 2) + pow(x[q2], 2) + pow(x[q3], 2));
-    dxdt[q0] = -0.5 * (x[q1]*x[P] + x[q2]*x[Q] + x[q3]*x[R]) + lambda * x[q0];
-    dxdt[q1] = +0.5 * (x[q0]*x[P] + x[q2]*x[R] - x[q3]*x[Q]) + lambda * x[q1];
-    dxdt[q2] = +0.5 * (x[q0]*x[Q] + x[q3]*x[P] - x[q1]*x[R]) + lambda * x[q2];
-    dxdt[q3] = +0.5 * (x[q0]*x[R] + x[q1]*x[Q] - x[q2]*x[P]) + lambda * x[q3];
+    dxdt[q0] = -0.5 * (x[q1] * x[P] + x[q2] * x[Q] + x[q3] * x[R]) + lambda * x[q0];
+    dxdt[q1] = +0.5 * (x[q0] * x[P] + x[q2] * x[R] - x[q3] * x[Q]) + lambda * x[q1];
+    dxdt[q2] = +0.5 * (x[q0] * x[Q] + x[q3] * x[P] - x[q1] * x[R]) + lambda * x[q2];
+    dxdt[q3] = +0.5 * (x[q0] * x[R] + x[q1] * x[Q] - x[q2] * x[P]) + lambda * x[q3];
 
     // Local position / velocity to global
     // Normalize quaternion
@@ -384,11 +366,11 @@ void Multirotor::model(const vector_t &x , vector_t &dxdt , double t) {
 
     //// TODO: Should these be cached from previous run or should the current dxdt
 
-    Eigen::Vector3d acc_local = F_total / mass_; // this is in body frame
+    Eigen::Vector3d acc_local = F_total / mass_;  // this is in body frame
     Eigen::Vector3d acc_world = quat.rotate(acc_local);
     dxdt[Uw] = acc_world(0);
     dxdt[Vw] = acc_world(1);
     dxdt[Ww] = acc_world(2);
 }
-} // namespace motion
-} // namespace scrimmage
+}  // namespace motion
+}  // namespace scrimmage
diff --git a/src/plugins/motion/RigidBody6DOF/RigidBody6DOF.cpp b/src/plugins/motion/RigidBody6DOF/RigidBody6DOF.cpp
index d9714e7563..d8552d6761 100644
--- a/src/plugins/motion/RigidBody6DOF/RigidBody6DOF.cpp
+++ b/src/plugins/motion/RigidBody6DOF/RigidBody6DOF.cpp
@@ -30,12 +30,12 @@
  *
  */
 
-#include <scrimmage/plugins/motion/RigidBody6DOF/RigidBody6DOF.h>
 #include <scrimmage/common/Utilities.h>
+#include <scrimmage/entity/Entity.h>
+#include <scrimmage/math/Angles.h>
 #include <scrimmage/parse/ParseUtils.h>
 #include <scrimmage/plugin_manager/RegisterPlugin.h>
-#include <scrimmage/math/Angles.h>
-#include <scrimmage/entity/Entity.h>
+#include <scrimmage/plugins/motion/RigidBody6DOF/RigidBody6DOF.h>
 
 #include <boost/algorithm/clamp.hpp>
 
@@ -49,42 +49,18 @@ namespace motion {
 namespace sc = scrimmage;
 namespace pl = std::placeholders;
 
-enum ModelParams {
-    U = 0,
-    V,
-    W,
-    P,
-    Q,
-    R,
-    Uw,
-    Vw,
-    Ww,
-    Xw,
-    Yw,
-    Zw,
-    q0,
-    q1,
-    q2,
-    q3,
-    MODEL_NUM_ITEMS
-};
-
-enum ControlParams {
-    THRUST = 0,
-    ELEVATOR,
-    AILERON,
-    RUDDER,
-    CONTROL_NUM_ITEMS
-};
+enum ModelParams { U = 0, V, W, P, Q, R, Uw, Vw, Ww, Xw, Yw, Zw, q0, q1, q2, q3, MODEL_NUM_ITEMS };
+
+enum ControlParams { THRUST = 0, ELEVATOR, AILERON, RUDDER, CONTROL_NUM_ITEMS };
 
 std::tuple<int, int, int> RigidBody6DOF::version() {
     return std::tuple<int, int, int>(0, 0, 1);
 }
 
-bool RigidBody6DOF::init(std::map<std::string, std::string> &info,
-                          std::map<std::string, std::string> &params) {
+bool RigidBody6DOF::init(std::map<std::string, std::string>& info,
+                         std::map<std::string, std::string>& params) {
     x_.resize(MODEL_NUM_ITEMS);
-    Eigen::Vector3d &pos = state_->pos();
+    Eigen::Vector3d& pos = state_->pos();
     quat_world_ = state_->quat();
 
     min_velocity_ = sc::get("min_velocity", params, 15.0);
@@ -158,7 +134,7 @@ bool RigidBody6DOF::step(double time, double dt) {
     return true;
 }
 
-void RigidBody6DOF::model(const vector_t &x , vector_t &dxdt , double t) {
+void RigidBody6DOF::model(const vector_t& x, vector_t& dxdt, double t) {
     double thrust = (*ctrl_u_)(THRUST);
     double elevator = (*ctrl_u_)(ELEVATOR);
     double aileron = (*ctrl_u_)(AILERON);
@@ -169,7 +145,7 @@ void RigidBody6DOF::model(const vector_t &x , vector_t &dxdt , double t) {
     double F_x = F_thrust;
 
     double m = 1.0;
-    double g = 0; // -9.8; no gravity
+    double g = 0;  // -9.8; no gravity
     double theta = 0;
 
     double Ixx = 1;
@@ -186,26 +162,27 @@ void RigidBody6DOF::model(const vector_t &x , vector_t &dxdt , double t) {
     double Q_dot = dxdt[Q];
     double R_dot = dxdt[R];
 
-    dxdt[U] = x[V]*x[R] - x[W]*x[Q] - g*sin(theta) + F_x / m;
-    dxdt[V] = 0; // x[W]*x[P] - x[U]*x[R] + g*sin(phi)*cos(theta) + F_y / m;
-    dxdt[W] = 0; // x[U]*x[Q] - x[V]*x[P] + g*cos(phi)*cos(theta) + F_z / m;
+    dxdt[U] = x[V] * x[R] - x[W] * x[Q] - g * sin(theta) + F_x / m;
+    dxdt[V] = 0;  // x[W]*x[P] - x[U]*x[R] + g*sin(phi)*cos(theta) + F_y / m;
+    dxdt[W] = 0;  // x[U]*x[Q] - x[V]*x[P] + g*cos(phi)*cos(theta) + F_z / m;
 
-    double L = aileron + Ixx*P_dot - Ixz*R_dot - Ixz*x[P]*x[Q] + (Izz - Iyy)*x[R]*x[Q];
-    double M = elevator + Iyy*Q_dot + (Ixx-Izz)*x[P]*x[R] + (Ixz*(pow(x[P], 2) - pow(x[R], 2)));
-    double N = rudder + Izz*R_dot - Ixz*P_dot + (Iyy-Ixx)*x[P]*x[Q] + Ixz*x[Q]*x[R];
+    double L = aileron + Ixx * P_dot - Ixz * R_dot - Ixz * x[P] * x[Q] + (Izz - Iyy) * x[R] * x[Q];
+    double M =
+        elevator + Iyy * Q_dot + (Ixx - Izz) * x[P] * x[R] + (Ixz * (pow(x[P], 2) - pow(x[R], 2)));
+    double N = rudder + Izz * R_dot - Ixz * P_dot + (Iyy - Ixx) * x[P] * x[Q] + Ixz * x[Q] * x[R];
 
-    double L_tic = L + Ixz*x[P]*x[Q] - (Izz-Iyy)*x[R]*x[Q];
-    double N_tic = N - (Iyy-Ixx)*x[P]*x[Q] - Ixz*x[R]*x[Q];
+    double L_tic = L + Ixz * x[P] * x[Q] - (Izz - Iyy) * x[R] * x[Q];
+    double N_tic = N - (Iyy - Ixx) * x[P] * x[Q] - Ixz * x[R] * x[Q];
 
-    dxdt[P] = (L_tic*Izz-N_tic*Ixz) / (Ixx*Izz - pow(Ixz, 2));
-    dxdt[Q] = (M - (Ixx - Izz)*x[P]*x[R]) - Ixz*(pow(x[P], 2)-pow(x[R], 2))/Iyy;
-    dxdt[R] = (N_tic*Ixx+L_tic*Ixz) / (Ixx*Izz - pow(Ixz, 2));
+    dxdt[P] = (L_tic * Izz - N_tic * Ixz) / (Ixx * Izz - pow(Ixz, 2));
+    dxdt[Q] = (M - (Ixx - Izz) * x[P] * x[R]) - Ixz * (pow(x[P], 2) - pow(x[R], 2)) / Iyy;
+    dxdt[R] = (N_tic * Ixx + L_tic * Ixz) / (Ixx * Izz - pow(Ixz, 2));
 
     double lambda = 1 - (pow(x[q0], 2) + pow(x[q1], 2) + pow(x[q2], 2) + pow(x[q3], 2));
-    dxdt[q0] = -0.5 * (x[q1]*x[P] + x[q2]*x[Q] + x[q3]*x[R]) + lambda * x[q0];
-    dxdt[q1] = +0.5 * (x[q0]*x[P] + x[q2]*x[R] - x[q3]*x[Q]) + lambda * x[q1];
-    dxdt[q2] = +0.5 * (x[q0]*x[Q] + x[q3]*x[P] - x[q1]*x[R]) + lambda * x[q2];
-    dxdt[q3] = +0.5 * (x[q0]*x[R] + x[q1]*x[Q] - x[q2]*x[P]) + lambda * x[q3];
+    dxdt[q0] = -0.5 * (x[q1] * x[P] + x[q2] * x[Q] + x[q3] * x[R]) + lambda * x[q0];
+    dxdt[q1] = +0.5 * (x[q0] * x[P] + x[q2] * x[R] - x[q3] * x[Q]) + lambda * x[q1];
+    dxdt[q2] = +0.5 * (x[q0] * x[Q] + x[q3] * x[P] - x[q1] * x[R]) + lambda * x[q2];
+    dxdt[q3] = +0.5 * (x[q0] * x[R] + x[q1] * x[Q] - x[q2] * x[P]) + lambda * x[q3];
 
     // Local position / velocity to global
     // Normalize quaternion
@@ -234,7 +211,7 @@ void RigidBody6DOF::model(const vector_t &x , vector_t &dxdt , double t) {
     dxdt[Ww] = acc_world(2);
 }
 
-void RigidBody6DOF::teleport(sc::StatePtr &state) {
+void RigidBody6DOF::teleport(sc::StatePtr& state) {
     // x_[X] = state->pos()[0];
     // x_[Y] = state->pos()[1];
     // x_[Z] = state->pos()[2];
@@ -243,5 +220,5 @@ void RigidBody6DOF::teleport(sc::StatePtr &state) {
     // x_[YAW] = state->quat().yaw();
     // x_[SPEED] = state->vel()[0];
 }
-} // namespace motion
-} // namespace scrimmage
+}  // namespace motion
+}  // namespace scrimmage
diff --git a/src/plugins/motion/SimpleAircraft/SimpleAircraft.cpp b/src/plugins/motion/SimpleAircraft/SimpleAircraft.cpp
index 4f6da81541..ae06d32a8b 100644
--- a/src/plugins/motion/SimpleAircraft/SimpleAircraft.cpp
+++ b/src/plugins/motion/SimpleAircraft/SimpleAircraft.cpp
@@ -32,8 +32,8 @@
 
 #include <scrimmage/common/Utilities.h>
 #include <scrimmage/entity/Entity.h>
-#include <scrimmage/math/State.h>
 #include <scrimmage/math/Angles.h>
+#include <scrimmage/math/State.h>
 #include <scrimmage/parse/ParseUtils.h>
 #include <scrimmage/plugin_manager/RegisterPlugin.h>
 #include <scrimmage/plugins/motion/SimpleAircraft/SimpleAircraft.h>
@@ -47,33 +47,19 @@ REGISTER_PLUGIN(scrimmage::MotionModel, scrimmage::motion::SimpleAircraft, Simpl
 namespace scrimmage {
 namespace motion {
 
-enum ModelParams {
-    X = 0,
-    Y,
-    Z,
-    ROLL,
-    PITCH,
-    YAW,
-    SPEED,
-    MODEL_NUM_ITEMS
-};
-
-enum ControlParams {
-    THRUST = 0,
-    TURN_RATE,
-    PITCH_RATE,
-    CONTROL_NUM_ITEMS
-};
+enum ModelParams { X = 0, Y, Z, ROLL, PITCH, YAW, SPEED, MODEL_NUM_ITEMS };
+
+enum ControlParams { THRUST = 0, TURN_RATE, PITCH_RATE, CONTROL_NUM_ITEMS };
 
 std::tuple<int, int, int> SimpleAircraft::version() {
     return std::tuple<int, int, int>(0, 0, 1);
 }
 
-bool SimpleAircraft::init(std::map<std::string, std::string> &info,
-                          std::map<std::string, std::string> &params) {
+bool SimpleAircraft::init(std::map<std::string, std::string>& info,
+                          std::map<std::string, std::string>& params) {
     x_.resize(MODEL_NUM_ITEMS);
-    Eigen::Vector3d &pos = state_->pos();
-    Quaternion &quat = state_->quat();
+    Eigen::Vector3d& pos = state_->pos();
+    Quaternion& quat = state_->quat();
 
     min_velocity_ = get("min_velocity", params, 15.0);
     max_velocity_ = get("max_velocity", params, 40.0);
@@ -82,7 +68,6 @@ bool SimpleAircraft::init(std::map<std::string, std::string> &info,
     max_pitch_rate_ = Angles::deg2rad(get("max_pitch_rate", params, 57.3));
     max_roll_rate_ = Angles::deg2rad(get("max_roll_rate", params, 57.3));
 
-
     x_[X] = pos(0);
     x_[Y] = pos(1);
     x_[Z] = pos(2);
@@ -92,8 +77,10 @@ bool SimpleAircraft::init(std::map<std::string, std::string> &info,
     x_[SPEED] = clamp(state_->vel().norm(), min_velocity_, max_velocity_);
 
     length_ = get("turning_radius", params, 50.0);
-    speedTarget_ = get("speed_target", params, 50.0);  // The "0" speed for adjusting the turning radius
-    lengthSlopePerSpeed_ = get("radius_slope_per_speed", params, 0.0);  // Enables adjusting the turning radius based on speed
+    speedTarget_ =
+        get("speed_target", params, 50.0);  // The "0" speed for adjusting the turning radius
+    lengthSlopePerSpeed_ = get("radius_slope_per_speed", params,
+                               0.0);  // Enables adjusting the turning radius based on speed
 
     state_->pos() << x_[X], x_[Y], x_[Z];
     state_->quat().set(-x_[ROLL], x_[PITCH], x_[YAW]);
@@ -116,12 +103,13 @@ bool SimpleAircraft::step(double time, double dt) {
 
     state_->pos() << x_[X], x_[Y], x_[Z];
     state_->quat().set(-x_[ROLL], x_[PITCH], x_[YAW]);
-    state_->vel() << x_[SPEED] * cos(x_[YAW]) * cos(x_[PITCH]), x_[SPEED] * sin(x_[YAW]) * cos(x_[PITCH]), x_[SPEED] * sin(x_[PITCH]);
+    state_->vel() << x_[SPEED] * cos(x_[YAW]) * cos(x_[PITCH]),
+        x_[SPEED] * sin(x_[YAW]) * cos(x_[PITCH]), x_[SPEED] * sin(x_[PITCH]);
 
     return true;
 }
 
-void SimpleAircraft::model(const vector_t &x , vector_t &dxdt , double t) {
+void SimpleAircraft::model(const vector_t& x, vector_t& dxdt, double t) {
     /// 0 : x-position
     /// 1 : y-position
     /// 2 : z-position
@@ -139,19 +127,19 @@ void SimpleAircraft::model(const vector_t &x , vector_t &dxdt , double t) {
     pitch_rate = clamp(pitch_rate, -max_pitch_rate_, max_pitch_rate_);
 
     double xy_speed = x[SPEED] * cos(x[PITCH]);
-    dxdt[X] = xy_speed*cos(x[YAW]);
-    dxdt[Y] = xy_speed*sin(x[YAW]);
-    dxdt[Z] = -sin(x[PITCH])*x[SPEED];
+    dxdt[X] = xy_speed * cos(x[YAW]);
+    dxdt[Y] = xy_speed * sin(x[YAW]);
+    dxdt[Z] = -sin(x[PITCH]) * x[SPEED];
     dxdt[ROLL] = roll_rate;
     dxdt[PITCH] = pitch_rate;
     // Adjust the length based on the speed
     double currentLength = length_ + lengthSlopePerSpeed_ * (x[SPEED] - speedTarget_);
-    dxdt[YAW] = x[SPEED]/currentLength*tan(x[ROLL]);
+    dxdt[YAW] = x[SPEED] / currentLength * tan(x[ROLL]);
 
-    dxdt[SPEED] = throttle/5;
+    dxdt[SPEED] = throttle / 5;
 }
 
-void SimpleAircraft::teleport(StatePtr &state) {
+void SimpleAircraft::teleport(StatePtr& state) {
     x_[X] = state->pos()[0];
     x_[Y] = state->pos()[1];
     x_[Z] = state->pos()[2];
diff --git a/src/plugins/motion/SimpleCar/SimpleCar.cpp b/src/plugins/motion/SimpleCar/SimpleCar.cpp
index 8604665700..f3e21b1905 100644
--- a/src/plugins/motion/SimpleCar/SimpleCar.cpp
+++ b/src/plugins/motion/SimpleCar/SimpleCar.cpp
@@ -45,23 +45,12 @@ REGISTER_PLUGIN(scrimmage::MotionModel, scrimmage::motion::SimpleCar, SimpleCar_
 namespace scrimmage {
 namespace motion {
 
-enum ModelParams {
-    X = 0,
-    Y,
-    Z,
-    Z_dot,
-    THETA,
-    MODEL_NUM_ITEMS
-};
-
-enum ControlParams {
-    FORWARD_VELOCITY = 0,
-    TURN_RATE,
-    CONTROL_NUM_ITEMS
-};
-
-bool SimpleCar::init(std::map<std::string, std::string> &info,
-                     std::map<std::string, std::string> &params) {
+enum ModelParams { X = 0, Y, Z, Z_dot, THETA, MODEL_NUM_ITEMS };
+
+enum ControlParams { FORWARD_VELOCITY = 0, TURN_RATE, CONTROL_NUM_ITEMS };
+
+bool SimpleCar::init(std::map<std::string, std::string>& info,
+                     std::map<std::string, std::string>& params) {
     x_.resize(MODEL_NUM_ITEMS);
     x_[X] = std::stod(info["x"]);
     x_[Y] = std::stod(info["y"]);
@@ -97,15 +86,14 @@ bool SimpleCar::step(double time, double dt) {
     /////////////////////
     // Save state
     // Simple velocity
-    state_->vel() << (x_[X] - prev_x) / dt, (x_[Y] - prev_y) / dt,
-        (x_[Z] - prev_z) / dt;
+    state_->vel() << (x_[X] - prev_x) / dt, (x_[Y] - prev_y) / dt, (x_[Z] - prev_z) / dt;
 
     state_->pos() << x_[X], x_[Y], x_[Z];
     state_->quat().set(0, 0, x_[THETA]);
     return true;
 }
 
-void SimpleCar::model(const vector_t &x , vector_t &dxdt , double t) {
+void SimpleCar::model(const vector_t& x, vector_t& dxdt, double t) {
     /// 0 : x-position
     /// 1 : y-position
     /// 2 : theta
@@ -114,9 +102,9 @@ void SimpleCar::model(const vector_t &x , vector_t &dxdt , double t) {
     const double theta_lim = M_PI / 4 - 0.0001;
     const double u_theta = clamp(vars_.input(input_turn_rate_idx_), -theta_lim, theta_lim);
 
-    dxdt[X] = u_vel*cos(x[THETA]);
-    dxdt[Y] = u_vel*sin(x[THETA]);
-    dxdt[THETA] = u_vel/length_*tan(u_theta);
+    dxdt[X] = u_vel * cos(x[THETA]);
+    dxdt[Y] = u_vel * sin(x[THETA]);
+    dxdt[THETA] = u_vel / length_ * tan(u_theta);
 
     if (enable_gravity_) {
         dxdt[Z] = x[Z_dot];
@@ -139,5 +127,5 @@ void SimpleCar::model(const vector_t &x , vector_t &dxdt , double t) {
         dxdt[Z] = 0;
     }
 }
-} // namespace motion
-} // namespace scrimmage
+}  // namespace motion
+}  // namespace scrimmage
diff --git a/src/plugins/motion/SimpleQuadrotor/SimpleQuadrotor.cpp b/src/plugins/motion/SimpleQuadrotor/SimpleQuadrotor.cpp
index 420611fb69..1d104100f9 100644
--- a/src/plugins/motion/SimpleQuadrotor/SimpleQuadrotor.cpp
+++ b/src/plugins/motion/SimpleQuadrotor/SimpleQuadrotor.cpp
@@ -30,12 +30,12 @@
  *
  */
 
-#include <scrimmage/plugins/motion/SimpleQuadrotor/SimpleQuadrotor.h>
 #include <scrimmage/common/Utilities.h>
-#include <scrimmage/parse/ParseUtils.h>
+#include <scrimmage/entity/Entity.h>
 #include <scrimmage/math/Angles.h>
+#include <scrimmage/parse/ParseUtils.h>
 #include <scrimmage/plugin_manager/RegisterPlugin.h>
-#include <scrimmage/entity/Entity.h>
+#include <scrimmage/plugins/motion/SimpleQuadrotor/SimpleQuadrotor.h>
 
 REGISTER_PLUGIN(scrimmage::MotionModel, scrimmage::motion::SimpleQuadrotor, SimpleQuadrotor_plugin)
 
@@ -46,28 +46,12 @@ namespace pl = std::placeholders;
 namespace sc = scrimmage;
 using ang = sc::Angles;
 
-enum ModelParams {
-    X = 0,
-    Y,
-    Z,
-    XDOT,
-    YDOT,
-    ZDOT,
-    YAW,
-    YAWDOT,
-    MODEL_NUM_ITEMS
-};
-
-enum ControlParams {
-    X_THRUST = 0,
-    Y_THRUST,
-    Z_THRUST,
-    TURN_RATE,
-    CONTROL_NUM_ITEMS
-};
-
-bool SimpleQuadrotor::init(std::map<std::string, std::string> &info,
-                           std::map<std::string, std::string> &params) {
+enum ModelParams { X = 0, Y, Z, XDOT, YDOT, ZDOT, YAW, YAWDOT, MODEL_NUM_ITEMS };
+
+enum ControlParams { X_THRUST = 0, Y_THRUST, Z_THRUST, TURN_RATE, CONTROL_NUM_ITEMS };
+
+bool SimpleQuadrotor::init(std::map<std::string, std::string>& info,
+                           std::map<std::string, std::string>& params) {
     x_.resize(MODEL_NUM_ITEMS);
     x_[X] = state_->pos()(0);
     x_[Y] = state_->pos()(1);
@@ -101,7 +85,7 @@ bool SimpleQuadrotor::step(double time, double dt) {
     return true;
 }
 
-void SimpleQuadrotor::model(const vector_t &x , vector_t &dxdt , double t) {
+void SimpleQuadrotor::model(const vector_t& x, vector_t& dxdt, double t) {
     /// 0 : x-position
     /// 1 : y-position
     /// 2 : z-position
@@ -111,7 +95,7 @@ void SimpleQuadrotor::model(const vector_t &x , vector_t &dxdt , double t) {
     /// 6 : yaw
     /// 7 : yaw-velocity
 
-    Eigen::Vector4d &u = std::static_pointer_cast<Controller>(parent_->controllers().back())->u();
+    Eigen::Vector4d& u = std::static_pointer_cast<Controller>(parent_->controllers().back())->u();
     double max_x_velocity = 15;
     double max_z_velocity = 10;
 
@@ -130,14 +114,14 @@ void SimpleQuadrotor::model(const vector_t &x , vector_t &dxdt , double t) {
         z_velocity = max_z_velocity;
     }
 
-    dxdt[X]      = x_velocity * cos(x[YAW]);
-    dxdt[Y]      = x_velocity * sin(x[YAW]);
-    dxdt[Z]      = z_velocity;
-    dxdt[XDOT]   = x_thrust;
-    dxdt[YDOT]   = y_thrust;
-    dxdt[ZDOT]   = z_thrust;
-    dxdt[YAW]    = x[YAWDOT];
+    dxdt[X] = x_velocity * cos(x[YAW]);
+    dxdt[Y] = x_velocity * sin(x[YAW]);
+    dxdt[Z] = z_velocity;
+    dxdt[XDOT] = x_thrust;
+    dxdt[YDOT] = y_thrust;
+    dxdt[ZDOT] = z_thrust;
+    dxdt[YAW] = x[YAWDOT];
     dxdt[YAWDOT] = turn_force;
 }
-} // namespace motion
-} // namespace scrimmage
+}  // namespace motion
+}  // namespace scrimmage
diff --git a/src/plugins/motion/SingleIntegrator/SingleIntegrator.cpp b/src/plugins/motion/SingleIntegrator/SingleIntegrator.cpp
index 0d40f4476d..963e19ee93 100644
--- a/src/plugins/motion/SingleIntegrator/SingleIntegrator.cpp
+++ b/src/plugins/motion/SingleIntegrator/SingleIntegrator.cpp
@@ -30,23 +30,22 @@
  *
  */
 
-#include <scrimmage/math/State.h>
 #include <scrimmage/math/Angles.h>
-#include <scrimmage/plugin_manager/RegisterPlugin.h>
+#include <scrimmage/math/State.h>
 #include <scrimmage/plugin_manager/PluginManager.h>
-
+#include <scrimmage/plugin_manager/RegisterPlugin.h>
 #include <scrimmage/plugins/motion/SingleIntegrator/SingleIntegrator.h>
 
 #include <cmath>
 
-REGISTER_PLUGIN(scrimmage::MotionModel, scrimmage::motion::SingleIntegrator, SingleIntegrator_plugin)
+REGISTER_PLUGIN(scrimmage::MotionModel, scrimmage::motion::SingleIntegrator,
+                SingleIntegrator_plugin)
 
 namespace scrimmage {
 namespace motion {
 
-bool SingleIntegrator::init(std::map<std::string, std::string> &info,
-                            std::map<std::string, std::string> &params) {
-
+bool SingleIntegrator::init(std::map<std::string, std::string>& info,
+                            std::map<std::string, std::string>& params) {
     override_heading_ = scrimmage::get<bool>("override_heading", params, false);
     max_speed_ = scrimmage::get<double>("max_speed", params, max_speed_);
 
@@ -55,10 +54,11 @@ bool SingleIntegrator::init(std::map<std::string, std::string> &info,
     vel_z_idx_ = vars_.declare(VariableIO::Type::velocity_z, VariableIO::Direction::In);
 
     if (override_heading_) {
-        desired_heading_idx_ = vars_.declare(VariableIO::Type::desired_heading, VariableIO::Direction::In);
+        desired_heading_idx_ =
+            vars_.declare(VariableIO::Type::desired_heading, VariableIO::Direction::In);
     }
 
-    auto get = [&](auto s) {return std::stod(info.at(s));};
+    auto get = [&](auto s) { return std::stod(info.at(s)); };
     state_->pos() << get("x"), get("y"), get("z");
 
     state_->vel() << Eigen::Vector3d::Zero();
@@ -68,11 +68,10 @@ bool SingleIntegrator::init(std::map<std::string, std::string> &info,
 }
 
 bool SingleIntegrator::step(double /*t*/, double dt) {
-    Eigen::Vector3d desired_vel(vars_.input(vel_x_idx_),
-                                vars_.input(vel_y_idx_),
+    Eigen::Vector3d desired_vel(vars_.input(vel_x_idx_), vars_.input(vel_y_idx_),
                                 vars_.input(vel_z_idx_));
 
-    Eigen::Vector3d &vel = state_->vel();
+    Eigen::Vector3d& vel = state_->vel();
 
     // If the max_speed is less than 0, directly apply the desired velocity,
     // otherwise, normalize the velocity and multiply by the max_speed.
@@ -96,5 +95,5 @@ bool SingleIntegrator::step(double /*t*/, double dt) {
     return true;
 }
 
-} // namespace motion
-} // namespace scrimmage
+}  // namespace motion
+}  // namespace scrimmage
diff --git a/src/plugins/motion/UUV6DOF/UUV6DOF.cpp b/src/plugins/motion/UUV6DOF/UUV6DOF.cpp
index a10fd27ab2..0d0509a715 100644
--- a/src/plugins/motion/UUV6DOF/UUV6DOF.cpp
+++ b/src/plugins/motion/UUV6DOF/UUV6DOF.cpp
@@ -31,27 +31,23 @@
  */
 
 #include <scrimmage/common/Utilities.h>
-#include <scrimmage/parse/ParseUtils.h>
-#include <scrimmage/parse/MissionParse.h>
+#include <scrimmage/entity/Entity.h>
 #include <scrimmage/math/Angles.h>
+#include <scrimmage/parse/MissionParse.h>
+#include <scrimmage/parse/ParseUtils.h>
 #include <scrimmage/plugin_manager/RegisterPlugin.h>
-#include <scrimmage/entity/Entity.h>
-
 #include <scrimmage/plugins/motion/UUV6DOF/UUV6DOF.h>
 
-#include <iostream>
-
 #include <boost/algorithm/clamp.hpp>
+#include <iostream>
 
+using boost::algorithm::clamp;
 using std::cout;
 using std::endl;
-using boost::algorithm::clamp;
 
 namespace sc = scrimmage;
 
-REGISTER_PLUGIN(scrimmage::MotionModel,
-                scrimmage::motion::UUV6DOF,
-                UUV6DOF_plugin)
+REGISTER_PLUGIN(scrimmage::MotionModel, scrimmage::motion::UUV6DOF, UUV6DOF_plugin)
 
 namespace scrimmage {
 namespace motion {
@@ -62,20 +58,20 @@ UUV6DOF::UUV6DOF() {
     x_.resize(MODEL_NUM_ITEMS);
 }
 
-bool UUV6DOF::init(std::map<std::string, std::string> &info,
-                     std::map<std::string, std::string> &params) {
+bool UUV6DOF::init(std::map<std::string, std::string>& info,
+                   std::map<std::string, std::string>& params) {
     throttle_idx_ = vars_.declare(VariableIO::Type::throttle, VariableIO::Direction::In);
     elevator_idx_ = vars_.declare(VariableIO::Type::elevator, VariableIO::Direction::In);
     rudder_idx_ = vars_.declare(VariableIO::Type::rudder, VariableIO::Direction::In);
 
-    Eigen::Vector3d &pos = state_->pos();
+    Eigen::Vector3d& pos = state_->pos();
 
     // Need to rotate axes by 180 degrees around X-axis SCRIMMAGE's global
     // frame uses Z-axis pointing up. Many aircraft equations of motion are
     // specified with Z-axis pointing down.
     quat_body_ = rot_180_x_axis_ * state_->quat();
-    quat_body_.set(sc::Angles::angle_pi(quat_body_.roll()+M_PI),
-                   quat_body_.pitch(), quat_body_.yaw());
+    quat_body_.set(sc::Angles::angle_pi(quat_body_.roll() + M_PI), quat_body_.pitch(),
+                   quat_body_.yaw());
 
     x_[U] = state_->vel()(0);
     x_[V] = 0;
@@ -114,8 +110,7 @@ bool UUV6DOF::init(std::map<std::string, std::string> &info,
     {
         // Get the inertia matrix
         std::vector<std::vector<std::string>> vecs;
-        std::string inertia_matrix = sc::get<std::string>("inertia_matrix",
-                                                          params, "");
+        std::string inertia_matrix = sc::get<std::string>("inertia_matrix", params, "");
         // Parse inertia matrix
         bool valid_inertia = false;
         if (!sc::get_vec_of_vecs(inertia_matrix, vecs)) {
@@ -177,22 +172,16 @@ bool UUV6DOF::init(std::map<std::string, std::string> &info,
     // Should we write a CSV file? What values should be written?
     write_csv_ = sc::get<bool>("write_csv", params, false);
     if (write_csv_) {
-        csv_.open_output(parent_->mp()->log_dir() + "/"
-                         + std::to_string(parent_->id().id())
+        csv_.open_output(parent_->mp()->log_dir() + "/" + std::to_string(parent_->id().id())
                          + "-states.csv");
         cout << "Writing log to " + parent_->mp()->log_dir() + "/"
-                         + std::to_string(parent_->id().id())
-                         + "-states.csv" << endl;
-
-        csv_.set_column_headers(sc::CSV::Headers{"t",
-                    "x", "y", "z",
-                    "U", "V", "W",
-                    "P", "Q", "R",
-                    "U_dot", "V_dot", "W_dot",
-                    "P_dot", "Q_dot", "R_dot",
-                    "roll", "pitch", "yaw",
-                    "throttle", "thrust",
-                    "elevator", "rudder"});
+                    + std::to_string(parent_->id().id()) + "-states.csv"
+             << endl;
+
+        csv_.set_column_headers(sc::CSV::Headers{
+            "t",    "x",     "y",     "z",        "U",      "V",        "W",     "P",
+            "Q",    "R",     "U_dot", "V_dot",    "W_dot",  "P_dot",    "Q_dot", "R_dot",
+            "roll", "pitch", "yaw",   "throttle", "thrust", "elevator", "rudder"});
     }
 
     Xuu_ = sc::get<double>("Xuu", params, Xuu_);
@@ -230,13 +219,10 @@ bool UUV6DOF::init(std::map<std::string, std::string> &info,
     double Iyy = I_(1, 1);
     double Izz = I_(2, 2);
 
-    masses_ <<
-        m-Xu_dot, 0          , 0           , 0         , m*zg        , -m*yg      ,
-        0       , m-Yv_dot   , 0           , -m*zg     , 0           , m*xg-Yr_dot,
-        0       , 0          , m-Zw_dot    , m*yg      , -m*xg-Zq_dot, 0          ,
-        0       , -m*zg      , m*yg        , Ixx-Kp_dot, 0           , 0          ,
-        m*zg    , 0          , -m*xg-Mw_dot, 0         , Iyy-Mq_dot  , 0          ,
-        -m*yg   , m*xg-Nv_dot, 0           , 0         , 0           , Izz-Nr_dot;
+    masses_ << m - Xu_dot, 0, 0, 0, m * zg, -m * yg, 0, m - Yv_dot, 0, -m * zg, 0, m * xg - Yr_dot,
+        0, 0, m - Zw_dot, m * yg, -m * xg - Zq_dot, 0, 0, -m * zg, m * yg, Ixx - Kp_dot, 0, 0,
+        m * zg, 0, -m * xg - Mw_dot, 0, Iyy - Mq_dot, 0, -m * yg, m * xg - Nv_dot, 0, 0, 0,
+        Izz - Nr_dot;
 
     masses_inverse_ = masses_.inverse();
 
@@ -250,7 +236,8 @@ bool UUV6DOF::step(double time, double dt) {
     Kprop_ = -scale<double>(throttle_, -1.0, 1.0, -Kprop_max_mag_, Kprop_max_mag_);
 
     delta_elevator_ = clamp(vars_.input(elevator_idx_), -1.0, 1.0);
-    delta_elevator_ = scale<double>(delta_elevator_, -1.0, 1.0, delta_elevator_min_, delta_elevator_max_);
+    delta_elevator_ =
+        scale<double>(delta_elevator_, -1.0, 1.0, delta_elevator_min_, delta_elevator_max_);
 
     delta_rudder_ = clamp(vars_.input(rudder_idx_), -1.0, 1.0);
     delta_rudder_ = scale<double>(delta_rudder_, -1.0, 1.0, delta_rudder_min_, delta_rudder_max_);
@@ -262,7 +249,7 @@ bool UUV6DOF::step(double time, double dt) {
     Eigen::Vector3d prev_angular_vel(x_[P], x_[Q], x_[R]);
 
     force_ext_body_ = quat_body_.rotate_reverse(ext_force_);
-    ext_force_ = Eigen::Vector3d::Zero(); // reset ext_force_ member variable
+    ext_force_ = Eigen::Vector3d::Zero();  // reset ext_force_ member variable
 
     ode_step(dt);
 
@@ -296,9 +283,8 @@ bool UUV6DOF::step(double time, double dt) {
 
     // Rotate back to Z-axis pointing up
     state_->quat() = rot_180_x_axis_ * quat_body_;
-    state_->quat().set(sc::Angles::angle_pi(state_->quat().roll()+M_PI),
-                       state_->quat().pitch(), state_->quat().yaw());
-
+    state_->quat().set(sc::Angles::angle_pi(state_->quat().roll() + M_PI), state_->quat().pitch(),
+                       state_->quat().yaw());
 
     Eigen::Vector3d angvel_b_e_bodyRef = quat_body_.rotate(angular_vel);
     Eigen::Vector3d angvel_b_e_ENU;
@@ -313,38 +299,37 @@ bool UUV6DOF::step(double time, double dt) {
 
     if (write_csv_) {
         // Log state to CSV
-        csv_.append(sc::CSV::Pairs{
-                {"t", time},
-                {"x", x_[Xw]},
-                {"y", x_[Yw]},
-                {"z", x_[Zw]},
-                {"U", x_[U]},
-                {"V", x_[V]},
-                {"W", x_[W]},
-                {"P", x_[P]},
-                {"Q", x_[Q]},
-                {"R", x_[R]},
-                {"U_dot", x_[U_dot]},
-                {"V_dot", x_[V_dot]},
-                {"W_dot", x_[W_dot]},
-                {"P_dot", x_[P_dot]},
-                {"Q_dot", x_[Q_dot]},
-                {"R_dot", x_[R_dot]},
-                {"roll", quat_body_.roll()},
-                {"pitch", quat_body_.pitch()},
-                {"yaw", quat_body_.yaw()},
-                {"throttle", throttle_},
-                {"thrust", thrust_},
-                {"elevator", delta_elevator_},
-                {"rudder", delta_rudder_}});
+        csv_.append(sc::CSV::Pairs{{"t", time},
+                                   {"x", x_[Xw]},
+                                   {"y", x_[Yw]},
+                                   {"z", x_[Zw]},
+                                   {"U", x_[U]},
+                                   {"V", x_[V]},
+                                   {"W", x_[W]},
+                                   {"P", x_[P]},
+                                   {"Q", x_[Q]},
+                                   {"R", x_[R]},
+                                   {"U_dot", x_[U_dot]},
+                                   {"V_dot", x_[V_dot]},
+                                   {"W_dot", x_[W_dot]},
+                                   {"P_dot", x_[P_dot]},
+                                   {"Q_dot", x_[Q_dot]},
+                                   {"R_dot", x_[R_dot]},
+                                   {"roll", quat_body_.roll()},
+                                   {"pitch", quat_body_.pitch()},
+                                   {"yaw", quat_body_.yaw()},
+                                   {"throttle", throttle_},
+                                   {"thrust", thrust_},
+                                   {"elevator", delta_elevator_},
+                                   {"rudder", delta_rudder_}});
     }
 
     return true;
 }
 
-void UUV6DOF::model(const vector_t &x, vector_t &dxdt, double t) {
+void UUV6DOF::model(const vector_t& x, vector_t& dxdt, double t) {
     // Calculate force from weight in body frame:
-    Eigen::Vector3d gravity_vector(0, 0, +mass_*g_);
+    Eigen::Vector3d gravity_vector(0, 0, +mass_ * g_);
     Eigen::Vector3d F_weight = quat_body_.rotate_reverse(gravity_vector);
 
     Eigen::Vector3d buoyancy_vector(0, 0, buoyancy_);
@@ -355,29 +340,33 @@ void UUV6DOF::model(const vector_t &x, vector_t &dxdt, double t) {
 
     double Xprop = thrust_;
 
-    double X_ext = F_hydro(0) + Xuu_ * x[U] * std::abs(x[U]) + Xu_dot*x[U_dot]
-        + Xwq*x[W]*x[Q] + Xqq*x[Q]*x[Q] + Xvr*x[V]*x[R] + Xrr*x[R]*x[R] + Xprop;
+    double X_ext = F_hydro(0) + Xuu_ * x[U] * std::abs(x[U]) + Xu_dot * x[U_dot] + Xwq * x[W] * x[Q]
+                   + Xqq * x[Q] * x[Q] + Xvr * x[V] * x[R] + Xrr * x[R] * x[R] + Xprop;
 
-    double Y_ext = F_hydro(1) + Yvv_*x[V]*std::abs(x[V]) + Yrr*x[R]*std::abs(x[R])
-        + Yv_dot*x[V_dot] + Yr_dot*x[R_dot] + Yur*x[U]*x[R] + Ywp*x[W]*x[P]
-        + Ypq*x[P]*x[Q] + Yuv*x[U]*x[V] + Yuu_delta_r * pow(x[U], 2) * delta_rudder_;
+    double Y_ext = F_hydro(1) + Yvv_ * x[V] * std::abs(x[V]) + Yrr * x[R] * std::abs(x[R])
+                   + Yv_dot * x[V_dot] + Yr_dot * x[R_dot] + Yur * x[U] * x[R] + Ywp * x[W] * x[P]
+                   + Ypq * x[P] * x[Q] + Yuv * x[U] * x[V]
+                   + Yuu_delta_r * pow(x[U], 2) * delta_rudder_;
 
-    double Z_ext = F_hydro(2) + Zww_*x[W]*std::abs(x[W]) + Zqq*x[Q]*std::abs(x[Q])
-        + Zw_dot*x[W_dot] + Zq_dot*x[Q_dot] + Zuq*x[U]*x[Q] + Zvp*x[V]*x[P]
-        + Zrp*x[R]*x[P] + Zuw*x[U]*x[W] + Zuu_delta_s*pow(x[U], 2) * delta_elevator_;
+    double Z_ext = F_hydro(2) + Zww_ * x[W] * std::abs(x[W]) + Zqq * x[Q] * std::abs(x[Q])
+                   + Zw_dot * x[W_dot] + Zq_dot * x[Q_dot] + Zuq * x[U] * x[Q] + Zvp * x[V] * x[P]
+                   + Zrp * x[R] * x[P] + Zuw * x[U] * x[W]
+                   + Zuu_delta_s * pow(x[U], 2) * delta_elevator_;
 
     // Roll moment
-    double K_ext = Moments_hydro(0) + Kpp*x[P]*std::abs(x[P]) + Kp_dot*x[P_dot] + Kprop_;
+    double K_ext = Moments_hydro(0) + Kpp * x[P] * std::abs(x[P]) + Kp_dot * x[P_dot] + Kprop_;
 
     // Pitch moment
-    double M_ext = Moments_hydro(1) + Mww_*x[W]*std::abs(x[W]) + Mqq_*x[Q]*std::abs(x[Q])
-        + Mw_dot*x[W_dot] + Mq_dot*x[Q_dot] + Muq*x[U]*x[Q] + Mvp*x[V]*x[P] + Mrp*x[R]*x[P]
-        + Muw*x[U]*x[W] + Muu_delta_s*pow(x[U], 2) * delta_elevator_;
+    double M_ext = Moments_hydro(1) + Mww_ * x[W] * std::abs(x[W]) + Mqq_ * x[Q] * std::abs(x[Q])
+                   + Mw_dot * x[W_dot] + Mq_dot * x[Q_dot] + Muq * x[U] * x[Q] + Mvp * x[V] * x[P]
+                   + Mrp * x[R] * x[P] + Muw * x[U] * x[W]
+                   + Muu_delta_s * pow(x[U], 2) * delta_elevator_;
 
     // Yaw moment
-    double N_ext = Moments_hydro(2) + Nvv*x[V]*std::abs(x[V]) + Nrr*x[R]*std::abs(x[R])
-        + Nv_dot*x[V_dot] + Nr_dot*x[R_dot] + Nur*x[U]*x[R] + Nwp*x[W]*x[P] + Npq*x[P]*x[Q]
-        + Nuv*x[U]*x[V] + Nuu_delta_r*pow(x[U], 2)*delta_rudder_;
+    double N_ext = Moments_hydro(2) + Nvv * x[V] * std::abs(x[V]) + Nrr * x[R] * std::abs(x[R])
+                   + Nv_dot * x[V_dot] + Nr_dot * x[R_dot] + Nur * x[U] * x[R] + Nwp * x[W] * x[P]
+                   + Npq * x[P] * x[Q] + Nuv * x[U] * x[V]
+                   + Nuu_delta_r * pow(x[U], 2) * delta_rudder_;
 
     Eigen::Matrix<double, 6, 1> forces;
     forces << X_ext, Y_ext, Z_ext, K_ext, M_ext, N_ext;
@@ -393,10 +382,10 @@ void UUV6DOF::model(const vector_t &x, vector_t &dxdt, double t) {
 
     // Compute quaternion derivatives
     double lambda = 1 - (pow(x[q0], 2) + pow(x[q1], 2) + pow(x[q2], 2) + pow(x[q3], 2));
-    dxdt[q0] = -0.5 * (x[q1]*x[P] + x[q2]*x[Q] + x[q3]*x[R]) + lambda * x[q0];
-    dxdt[q1] = +0.5 * (x[q0]*x[P] + x[q2]*x[R] - x[q3]*x[Q]) + lambda * x[q1];
-    dxdt[q2] = +0.5 * (x[q0]*x[Q] + x[q3]*x[P] - x[q1]*x[R]) + lambda * x[q2];
-    dxdt[q3] = +0.5 * (x[q0]*x[R] + x[q1]*x[Q] - x[q2]*x[P]) + lambda * x[q3];
+    dxdt[q0] = -0.5 * (x[q1] * x[P] + x[q2] * x[Q] + x[q3] * x[R]) + lambda * x[q0];
+    dxdt[q1] = +0.5 * (x[q0] * x[P] + x[q2] * x[R] - x[q3] * x[Q]) + lambda * x[q1];
+    dxdt[q2] = +0.5 * (x[q0] * x[Q] + x[q3] * x[P] - x[q1] * x[R]) + lambda * x[q2];
+    dxdt[q3] = +0.5 * (x[q0] * x[R] + x[q1] * x[Q] - x[q2] * x[P]) + lambda * x[q3];
 
     // Normalize quaternion
     sc::Quaternion quat(x[q0], x[q1], x[q2], x[q3]);
@@ -408,17 +397,17 @@ void UUV6DOF::model(const vector_t &x, vector_t &dxdt, double t) {
 
     // Integrate local velocities to compute local positions
     Eigen::Vector3d vel_local(x[U], x[V], x[W]);
-    Eigen::Vector3d vel_world = quat.rotate(vel_local); // rot * vel_local;
+    Eigen::Vector3d vel_world = quat.rotate(vel_local);  // rot * vel_local;
     dxdt[Xw] = vel_world(0);
-    dxdt[Yw] = -vel_world(1); // Due to rotated frame
-    dxdt[Zw] = -vel_world(2); // Due to rotated frame
+    dxdt[Yw] = -vel_world(1);  // Due to rotated frame
+    dxdt[Zw] = -vel_world(2);  // Due to rotated frame
 
     // // Integrate local accelerations to compute global velocities
     Eigen::Vector3d acc_local = forces.head<3>() / mass_;
-    Eigen::Vector3d acc_world = quat.rotate(acc_local); // rot * acc_local;
+    Eigen::Vector3d acc_world = quat.rotate(acc_local);  // rot * acc_local;
     dxdt[Uw] = acc_world(0);
-    dxdt[Vw] = -acc_world(1); // Due to rotated frame
-    dxdt[Ww] = -acc_world(2); // Due to rotated frame
+    dxdt[Vw] = -acc_world(1);  // Due to rotated frame
+    dxdt[Ww] = -acc_world(2);  // Due to rotated frame
     //
     // Accelerations get updated based on change in velocities
     dxdt[U_dot] = 0;
@@ -428,5 +417,5 @@ void UUV6DOF::model(const vector_t &x, vector_t &dxdt, double t) {
     dxdt[Q_dot] = 0;
     dxdt[R_dot] = 0;
 }
-} // namespace motion
-} // namespace scrimmage
+}  // namespace motion
+}  // namespace scrimmage
diff --git a/src/plugins/motion/Unicycle/Unicycle.cpp b/src/plugins/motion/Unicycle/Unicycle.cpp
index a29e9cb0bd..fc6a0ffd61 100644
--- a/src/plugins/motion/Unicycle/Unicycle.cpp
+++ b/src/plugins/motion/Unicycle/Unicycle.cpp
@@ -30,12 +30,12 @@
  *
  */
 
-#include <scrimmage/plugins/motion/Unicycle/Unicycle.h>
 #include <scrimmage/common/Utilities.h>
-#include <scrimmage/parse/ParseUtils.h>
-#include <scrimmage/plugin_manager/RegisterPlugin.h>
 #include <scrimmage/entity/Entity.h>
 #include <scrimmage/math/State.h>
+#include <scrimmage/parse/ParseUtils.h>
+#include <scrimmage/plugin_manager/RegisterPlugin.h>
+#include <scrimmage/plugins/motion/Unicycle/Unicycle.h>
 
 #include <boost/algorithm/clamp.hpp>
 
@@ -43,21 +43,13 @@ REGISTER_PLUGIN(scrimmage::MotionModel, scrimmage::motion::Unicycle, Unicycle_pl
 
 using boost::algorithm::clamp;
 
-enum ModelParams {
-    X = 0,
-    Y,
-    Z,
-    YAW,
-    PITCH,
-    MODEL_NUM_ITEMS
-};
+enum ModelParams { X = 0, Y, Z, YAW, PITCH, MODEL_NUM_ITEMS };
 
 namespace scrimmage {
 namespace motion {
 
-bool Unicycle::init(std::map<std::string, std::string> &info,
-                    std::map<std::string, std::string> &params) {
-
+bool Unicycle::init(std::map<std::string, std::string>& info,
+                    std::map<std::string, std::string>& params) {
     // Declare variables for controllers
     use_pitch_ = get<bool>("use_pitch", params, use_pitch_);
 
@@ -118,7 +110,7 @@ bool Unicycle::step(double t, double dt) {
     state_->pos()(1) = x_[Y];
     state_->pos()(2) = x_[Z];
 
-    double roll = 0; // TODO: simulate roll if enabled
+    double roll = 0;  // TODO: simulate roll if enabled
     if (enable_roll_ && turn_rate_ != 0) {
         // see https://en.wikipedia.org/wiki/Standard_rate_turn
         const double radius = velocity_ / turn_rate_;
@@ -129,7 +121,7 @@ bool Unicycle::step(double t, double dt) {
     return true;
 }
 
-void Unicycle::model(const vector_t &x , vector_t &dxdt , double t) {
+void Unicycle::model(const vector_t& x, vector_t& dxdt, double t) {
     double xy_speed = velocity_ * cos(x[PITCH]);
     dxdt[X] = xy_speed * cos(x[YAW]);
     dxdt[Y] = xy_speed * sin(x[YAW]);
@@ -141,5 +133,5 @@ void Unicycle::model(const vector_t &x , vector_t &dxdt , double t) {
         dxdt[Z] = velocity_z_;
     }
 }
-} // namespace motion
-} // namespace scrimmage
+}  // namespace motion
+}  // namespace scrimmage
diff --git a/src/plugins/motion/Unicycle3D/Unicycle3D.cpp b/src/plugins/motion/Unicycle3D/Unicycle3D.cpp
index fa90512bd2..cd56d13afd 100644
--- a/src/plugins/motion/Unicycle3D/Unicycle3D.cpp
+++ b/src/plugins/motion/Unicycle3D/Unicycle3D.cpp
@@ -30,13 +30,14 @@
  *
  */
 
-#include <scrimmage/plugins/motion/Unicycle3D/Unicycle3D.h>
 #include <scrimmage/common/Utilities.h>
-#include <scrimmage/parse/ParseUtils.h>
-#include <scrimmage/parse/MissionParse.h>
-#include <scrimmage/plugin_manager/RegisterPlugin.h>
 #include <scrimmage/entity/Entity.h>
 #include <scrimmage/math/State.h>
+#include <scrimmage/parse/MissionParse.h>
+#include <scrimmage/parse/ParseUtils.h>
+#include <scrimmage/plugin_manager/RegisterPlugin.h>
+#include <scrimmage/plugins/motion/Unicycle3D/Unicycle3D.h>
+
 #include <boost/algorithm/clamp.hpp>
 
 REGISTER_PLUGIN(scrimmage::MotionModel, scrimmage::motion::Unicycle3D, Unicycle3D_plugin)
@@ -52,9 +53,9 @@ enum ModelParams {
     U = 0,
     V,
     W,
-    P, // roll rate
-    Q, // pitch rate
-    R, // yaw rate
+    P,  // roll rate
+    Q,  // pitch rate
+    R,  // yaw rate
     Uw,
     Vw,
     Ww,
@@ -68,11 +69,9 @@ enum ModelParams {
     MODEL_NUM_ITEMS
 };
 
-bool Unicycle3D::init(std::map<std::string, std::string> &info,
-                      std::map<std::string, std::string> &params) {
-
-    auto get_max_min = [&params] (const std::string &str, double &max,
-                                  double &min) {
+bool Unicycle3D::init(std::map<std::string, std::string>& info,
+                      std::map<std::string, std::string>& params) {
+    auto get_max_min = [&params](const std::string& str, double& max, double& min) {
         max = sc::get<double>(str + "_max", params, max);
         min = sc::get<double>(str + "_min", params, -max);
         if (max < min) {
@@ -104,7 +103,7 @@ bool Unicycle3D::init(std::map<std::string, std::string> &info,
 
     // Setup model size and quaternions
     x_.resize(MODEL_NUM_ITEMS);
-    Eigen::Vector3d &pos = state_->pos();
+    Eigen::Vector3d& pos = state_->pos();
     quat_world_ = state_->quat();
     quat_world_.normalize();
 
@@ -140,27 +139,25 @@ bool Unicycle3D::init(std::map<std::string, std::string> &info,
 
     write_csv_ = sc::get<bool>("write_csv", params, false);
     if (write_csv_) {
-        csv_.open_output(parent_->mp()->log_dir() + "/"
-                         + std::to_string(parent_->id().id())
+        csv_.open_output(parent_->mp()->log_dir() + "/" + std::to_string(parent_->id().id())
                          + "-unicycle-states.csv");
 
-        csv_.set_column_headers(sc::CSV::Headers{"t",
-                        "x", "y", "z",
-                        "U", "V", "W",
-                        "P", "Q", "R",
-                        "roll", "pitch", "yaw",
-                        "speed",
-                        "turn_rate", "pitch_rate", "roll_rate",
-                        "Uw", "Vw", "Ww"});
+        csv_.set_column_headers(sc::CSV::Headers{
+            "t",         "x",          "y",         "z",    "U",     "V",   "W",
+            "P",         "Q",          "R",         "roll", "pitch", "yaw", "speed",
+            "turn_rate", "pitch_rate", "roll_rate", "Uw",   "Vw",    "Ww"});
     }
     return true;
 }
 
 bool Unicycle3D::step(double t, double dt) {
     // Get inputs and saturate
-    turn_rate_ = boost::algorithm::clamp(vars_.input(turn_rate_idx_), turn_rate_min_, turn_rate_max_);
-    pitch_rate_ = boost::algorithm::clamp(vars_.input(pitch_rate_idx_), pitch_rate_min_, pitch_rate_max_);
-    roll_rate_ = boost::algorithm::clamp(vars_.input(roll_rate_idx_), roll_rate_min_, roll_rate_max_);
+    turn_rate_ =
+        boost::algorithm::clamp(vars_.input(turn_rate_idx_), turn_rate_min_, turn_rate_max_);
+    pitch_rate_ =
+        boost::algorithm::clamp(vars_.input(pitch_rate_idx_), pitch_rate_min_, pitch_rate_max_);
+    roll_rate_ =
+        boost::algorithm::clamp(vars_.input(roll_rate_idx_), roll_rate_min_, roll_rate_max_);
 
     x_[Uw] = state_->vel()(0);
     x_[Vw] = state_->vel()(1);
@@ -226,32 +223,31 @@ bool Unicycle3D::step(double t, double dt) {
 
     if (write_csv_) {
         // Log state to CSV
-        csv_.append(sc::CSV::Pairs{
-                {"t", t},
-                {"x", x_[Xw]},
-                {"y", x_[Yw]},
-                {"z", x_[Zw]},
-                {"U", x_[U]},
-                {"V", x_[V]},
-                {"W", x_[W]},
-                {"P", x_[P]},
-                {"Q", x_[Q]},
-                {"R", x_[R]},
-                {"roll", state_->quat().roll()},
-                {"pitch", state_->quat().pitch()},
-                {"yaw", state_->quat().yaw()},
-                {"speed", speed_},
-                {"turn_rate", turn_rate_},
-                {"pitch_rate", pitch_rate_},
-                {"roll_rate", roll_rate_},
-                {"Uw", state_->vel()(0)},
-                {"Vw", state_->vel()(1)},
-                {"Ww", state_->vel()(2)}});
+        csv_.append(sc::CSV::Pairs{{"t", t},
+                                   {"x", x_[Xw]},
+                                   {"y", x_[Yw]},
+                                   {"z", x_[Zw]},
+                                   {"U", x_[U]},
+                                   {"V", x_[V]},
+                                   {"W", x_[W]},
+                                   {"P", x_[P]},
+                                   {"Q", x_[Q]},
+                                   {"R", x_[R]},
+                                   {"roll", state_->quat().roll()},
+                                   {"pitch", state_->quat().pitch()},
+                                   {"yaw", state_->quat().yaw()},
+                                   {"speed", speed_},
+                                   {"turn_rate", turn_rate_},
+                                   {"pitch_rate", pitch_rate_},
+                                   {"roll_rate", roll_rate_},
+                                   {"Uw", state_->vel()(0)},
+                                   {"Vw", state_->vel()(1)},
+                                   {"Ww", state_->vel()(2)}});
     }
     return true;
 }
 
-void Unicycle3D::model(const vector_t &x , vector_t &dxdt , double t) {
+void Unicycle3D::model(const vector_t& x, vector_t& dxdt, double t) {
     dxdt[U] = acceleration_;
     dxdt[V] = 0;
     dxdt[W] = 0;
@@ -261,10 +257,10 @@ void Unicycle3D::model(const vector_t &x , vector_t &dxdt , double t) {
     dxdt[R] = 0;
 
     double lambda = 1 - (pow(x[q0], 2) + pow(x[q1], 2) + pow(x[q2], 2) + pow(x[q3], 2));
-    dxdt[q0] = -0.5 * (x[q1]*x[P] + x[q2]*x[Q] + x[q3]*x[R]) + lambda * x[q0];
-    dxdt[q1] = +0.5 * (x[q0]*x[P] + x[q2]*x[R] - x[q3]*x[Q]) + lambda * x[q1];
-    dxdt[q2] = +0.5 * (x[q0]*x[Q] + x[q3]*x[P] - x[q1]*x[R]) + lambda * x[q2];
-    dxdt[q3] = +0.5 * (x[q0]*x[R] + x[q1]*x[Q] - x[q2]*x[P]) + lambda * x[q3];
+    dxdt[q0] = -0.5 * (x[q1] * x[P] + x[q2] * x[Q] + x[q3] * x[R]) + lambda * x[q0];
+    dxdt[q1] = +0.5 * (x[q0] * x[P] + x[q2] * x[R] - x[q3] * x[Q]) + lambda * x[q1];
+    dxdt[q2] = +0.5 * (x[q0] * x[Q] + x[q3] * x[P] - x[q1] * x[R]) + lambda * x[q2];
+    dxdt[q3] = +0.5 * (x[q0] * x[R] + x[q1] * x[Q] - x[q2] * x[P]) + lambda * x[q3];
 
     // Local position / velocity to global
     // Normalize quaternion
@@ -283,5 +279,5 @@ void Unicycle3D::model(const vector_t &x , vector_t &dxdt , double t) {
     dxdt[Yw] = vel_world(1);
     dxdt[Zw] = vel_world(2);
 }
-} // namespace motion
-} // namespace scrimmage
+}  // namespace motion
+}  // namespace scrimmage
diff --git a/src/plugins/network/GlobalNetwork/GlobalNetwork.cpp b/src/plugins/network/GlobalNetwork/GlobalNetwork.cpp
index 82345df320..380f32c605 100644
--- a/src/plugins/network/GlobalNetwork/GlobalNetwork.cpp
+++ b/src/plugins/network/GlobalNetwork/GlobalNetwork.cpp
@@ -30,32 +30,29 @@
  *
  */
 
-#include <scrimmage/plugins/network/GlobalNetwork/GlobalNetwork.h>
-
 #include <scrimmage/common/ID.h>
-#include <scrimmage/common/Time.h>
-#include <scrimmage/common/Random.h>
 #include <scrimmage/common/RTree.h>
+#include <scrimmage/common/Random.h>
+#include <scrimmage/common/Time.h>
 #include <scrimmage/entity/Entity.h>
 #include <scrimmage/math/State.h>
 #include <scrimmage/plugin_manager/RegisterPlugin.h>
+#include <scrimmage/plugins/network/GlobalNetwork/GlobalNetwork.h>
 #include <scrimmage/plugins/network/SphereNetwork/SphereNetwork.h>
+#include <scrimmage/pubsub/Message.h>
 #include <scrimmage/pubsub/Publisher.h>
 #include <scrimmage/pubsub/Subscriber.h>
-#include <scrimmage/pubsub/Message.h>
 
-#include <memory>
-#include <limits>
 #include <iostream>
+#include <limits>
+#include <memory>
 
 using std::cout;
 using std::endl;
 
 namespace sc = scrimmage;
 
-REGISTER_PLUGIN(scrimmage::Network,
-                scrimmage::network::GlobalNetwork,
-                GlobalNetwork_plugin)
+REGISTER_PLUGIN(scrimmage::Network, scrimmage::network::GlobalNetwork, GlobalNetwork_plugin)
 
 namespace scrimmage {
 namespace network {
@@ -63,22 +60,21 @@ namespace network {
 GlobalNetwork::GlobalNetwork() {
 }
 
-bool GlobalNetwork::init(std::map<std::string, std::string> &mission_params,
-                               std::map<std::string, std::string> &plugin_params) {
+bool GlobalNetwork::init(std::map<std::string, std::string>& mission_params,
+                         std::map<std::string, std::string>& plugin_params) {
     network_init(mission_params, plugin_params);
     return true;
 }
 
-bool GlobalNetwork::is_reachable(const scrimmage::EntityPluginPtr &pub_plugin,
-                                       const scrimmage::EntityPluginPtr &sub_plugin) {
+bool GlobalNetwork::is_reachable(const scrimmage::EntityPluginPtr& pub_plugin,
+                                 const scrimmage::EntityPluginPtr& sub_plugin) {
     return true;
 }
 
-bool GlobalNetwork::is_successful_transmission(const scrimmage::EntityPluginPtr &pub_plugin,
-                                                     const scrimmage::EntityPluginPtr &sub_plugin) {
+bool GlobalNetwork::is_successful_transmission(const scrimmage::EntityPluginPtr& pub_plugin,
+                                               const scrimmage::EntityPluginPtr& sub_plugin) {
     return true;
 }
 
-
-} // namespace network
-} // namespace scrimmage
+}  // namespace network
+}  // namespace scrimmage
diff --git a/src/plugins/network/LocalNetwork/LocalNetwork.cpp b/src/plugins/network/LocalNetwork/LocalNetwork.cpp
index 09beef1572..b060904ad3 100644
--- a/src/plugins/network/LocalNetwork/LocalNetwork.cpp
+++ b/src/plugins/network/LocalNetwork/LocalNetwork.cpp
@@ -30,32 +30,29 @@
  *
  */
 
-#include <scrimmage/plugins/network/LocalNetwork/LocalNetwork.h>
-
 #include <scrimmage/common/ID.h>
-#include <scrimmage/common/Time.h>
-#include <scrimmage/common/Random.h>
 #include <scrimmage/common/RTree.h>
+#include <scrimmage/common/Random.h>
+#include <scrimmage/common/Time.h>
 #include <scrimmage/entity/Entity.h>
 #include <scrimmage/math/State.h>
 #include <scrimmage/plugin_manager/RegisterPlugin.h>
+#include <scrimmage/plugins/network/LocalNetwork/LocalNetwork.h>
 #include <scrimmage/plugins/network/SphereNetwork/SphereNetwork.h>
+#include <scrimmage/pubsub/Message.h>
 #include <scrimmage/pubsub/Publisher.h>
 #include <scrimmage/pubsub/Subscriber.h>
-#include <scrimmage/pubsub/Message.h>
 
-#include <memory>
-#include <limits>
 #include <iostream>
+#include <limits>
+#include <memory>
 
 using std::cout;
 using std::endl;
 
 namespace sc = scrimmage;
 
-REGISTER_PLUGIN(scrimmage::Network,
-                scrimmage::network::LocalNetwork,
-                LocalNetwork_plugin)
+REGISTER_PLUGIN(scrimmage::Network, scrimmage::network::LocalNetwork, LocalNetwork_plugin)
 
 namespace scrimmage {
 namespace network {
@@ -63,25 +60,24 @@ namespace network {
 LocalNetwork::LocalNetwork() {
 }
 
-bool LocalNetwork::init(std::map<std::string, std::string> &mission_params,
-                        std::map<std::string, std::string> &plugin_params) {
+bool LocalNetwork::init(std::map<std::string, std::string>& mission_params,
+                        std::map<std::string, std::string>& plugin_params) {
     network_init(mission_params, plugin_params);
     return true;
 }
 
-bool LocalNetwork::is_reachable(const scrimmage::EntityPluginPtr &pub_plugin,
-                                const scrimmage::EntityPluginPtr &sub_plugin) {
+bool LocalNetwork::is_reachable(const scrimmage::EntityPluginPtr& pub_plugin,
+                                const scrimmage::EntityPluginPtr& sub_plugin) {
     // Never reachable if plugin's entity was destroyed
     if (pub_plugin->parent() == nullptr || sub_plugin->parent() == nullptr) return false;
     // If the publisher and subscriber have the same parent, it is reachable
     return (pub_plugin->parent() == sub_plugin->parent());
 }
 
-bool LocalNetwork::is_successful_transmission(const scrimmage::EntityPluginPtr &pub_plugin,
-                                              const scrimmage::EntityPluginPtr &sub_plugin) {
+bool LocalNetwork::is_successful_transmission(const scrimmage::EntityPluginPtr& pub_plugin,
+                                              const scrimmage::EntityPluginPtr& sub_plugin) {
     return true;
 }
 
-
-} // namespace network
-} // namespace scrimmage
+}  // namespace network
+}  // namespace scrimmage
diff --git a/src/plugins/network/SphereNetwork/SphereNetwork.cpp b/src/plugins/network/SphereNetwork/SphereNetwork.cpp
index cb201169f0..7d492f0da3 100644
--- a/src/plugins/network/SphereNetwork/SphereNetwork.cpp
+++ b/src/plugins/network/SphereNetwork/SphereNetwork.cpp
@@ -31,25 +31,23 @@
  */
 
 #include <scrimmage/common/ID.h>
-#include <scrimmage/common/Time.h>
-#include <scrimmage/common/Random.h>
 #include <scrimmage/common/RTree.h>
+#include <scrimmage/common/Random.h>
+#include <scrimmage/common/Time.h>
 #include <scrimmage/entity/Entity.h>
 #include <scrimmage/math/State.h>
+#include <scrimmage/parse/ParseUtils.h>
 #include <scrimmage/plugin_manager/RegisterPlugin.h>
 #include <scrimmage/plugins/network/SphereNetwork/SphereNetwork.h>
+#include <scrimmage/pubsub/Message.h>
 #include <scrimmage/pubsub/Publisher.h>
 #include <scrimmage/pubsub/Subscriber.h>
-#include <scrimmage/pubsub/Message.h>
-#include <scrimmage/parse/ParseUtils.h>
 
+#include <boost/range/adaptor/map.hpp>
 #include <iostream>
 #include <vector>
-#include <boost/range/adaptor/map.hpp>
 
-REGISTER_PLUGIN(scrimmage::Network,
-                scrimmage::network::SphereNetwork,
-                SphereNetwork_plugin)
+REGISTER_PLUGIN(scrimmage::Network, scrimmage::network::SphereNetwork, SphereNetwork_plugin)
 
 using std::cout;
 using std::endl;
@@ -60,25 +58,24 @@ namespace ba = boost::adaptors;
 namespace scrimmage {
 namespace network {
 
-bool SphereNetwork::init(std::map<std::string, std::string> &mission_params,
-                         std::map<std::string, std::string> &plugin_params) {
+bool SphereNetwork::init(std::map<std::string, std::string>& mission_params,
+                         std::map<std::string, std::string>& plugin_params) {
     network_init(mission_params, plugin_params);
 
     range_ = std::stod(plugin_params.at("range"));
     prob_transmit_ = std::stod(plugin_params.at("prob_transmit"));
-    filter_comms_plane_ = sc::get<bool>("filter_comms_plane", plugin_params,
-            filter_comms_plane_);
+    filter_comms_plane_ = sc::get<bool>("filter_comms_plane", plugin_params, filter_comms_plane_);
     if (filter_comms_plane_) {
-        comms_boundary_altitude_ = sc::get<double>("comms_boundary_altitude",
-                plugin_params, comms_boundary_altitude_);
-        comms_boundary_epsilon_ = sc::get<double>("comms_boundary_epsilon",
-                plugin_params, comms_boundary_epsilon_);
+        comms_boundary_altitude_ =
+            sc::get<double>("comms_boundary_altitude", plugin_params, comms_boundary_altitude_);
+        comms_boundary_epsilon_ =
+            sc::get<double>("comms_boundary_epsilon", plugin_params, comms_boundary_epsilon_);
     }
     return true;
 }
 
-bool SphereNetwork::is_reachable(const scrimmage::EntityPluginPtr &pub_plugin,
-                                 const scrimmage::EntityPluginPtr &sub_plugin) {
+bool SphereNetwork::is_reachable(const scrimmage::EntityPluginPtr& pub_plugin,
+                                 const scrimmage::EntityPluginPtr& sub_plugin) {
     // Never reachable if plugin's entity was destroyed
     if (pub_plugin->parent() == nullptr || sub_plugin->parent() == nullptr) return false;
     // If the publisher and subscriber have the same parent, it is reachable
@@ -100,26 +97,24 @@ bool SphereNetwork::is_reachable(const scrimmage::EntityPluginPtr &pub_plugin,
 
     // No reachability mapping exists. Find all entity IDs that are reachable.
     std::vector<sc::ID> neigh;
-    rtree_->neighbors_in_range(pub_plugin->parent()->state_truth()->pos(),
-                               neigh, range_);
+    rtree_->neighbors_in_range(pub_plugin->parent()->state_truth()->pos(), neigh, range_);
 
     // Add the entity IDs to the publisher's reachability set
     // Look for the subscriber ID
-    reachable_map_[pub_id][sub_id] = false; // Default to not reachable
-    reachable_map_[sub_id][pub_id] = false; // Default to not reachable
+    reachable_map_[pub_id][sub_id] = false;  // Default to not reachable
+    reachable_map_[sub_id][pub_id] = false;  // Default to not reachable
     for (sc::ID id : neigh) {
         auto ent_neighbor = id_to_ent_map_->find(id.id());
         if (ent_neighbor == id_to_ent_map_->end()) {
-            std::cout << "Warning: Sphere Network entity id doesn't exists: "
-                << id.id() << std::endl;
+            std::cout << "Warning: Sphere Network entity id doesn't exists: " << id.id()
+                      << std::endl;
             continue;
         }
         if (not filter_comms_plane_) {
             reachable_map_[pub_id][id.id()] = true;
             reachable_map_[id.id()][pub_id] = true;
-        } else if (within_planar_boundary(
-            pub_plugin->parent()->state_truth()->pos()[2],
-            ent_neighbor->second->state_truth()->pos()[2])) {
+        } else if (within_planar_boundary(pub_plugin->parent()->state_truth()->pos()[2],
+                                          ent_neighbor->second->state_truth()->pos()[2])) {
             reachable_map_[pub_id][id.id()] = true;
             reachable_map_[id.id()][pub_id] = true;
         }
@@ -127,19 +122,19 @@ bool SphereNetwork::is_reachable(const scrimmage::EntityPluginPtr &pub_plugin,
     return reachable_map_[pub_id][sub_id];
 }
 
-bool SphereNetwork::is_successful_transmission(const scrimmage::EntityPluginPtr &pub_plugin,
-                                               const scrimmage::EntityPluginPtr &sub_plugin) {
+bool SphereNetwork::is_successful_transmission(const scrimmage::EntityPluginPtr& pub_plugin,
+                                               const scrimmage::EntityPluginPtr& sub_plugin) {
     return (random_->rng_uniform(0, 1) <= prob_transmit_);
 }
 
 // Return true if the plane does not block communications
 bool SphereNetwork::within_planar_boundary(double z1, double z2) {
     bool both_above = (z1 >= (comms_boundary_altitude_ - comms_boundary_epsilon_))
-        && (z2 >= (comms_boundary_altitude_ - comms_boundary_epsilon_));
+                      && (z2 >= (comms_boundary_altitude_ - comms_boundary_epsilon_));
     bool both_below = (z1 <= (comms_boundary_altitude_ + comms_boundary_epsilon_))
-        && (z2 <= (comms_boundary_altitude_ + comms_boundary_epsilon_));
+                      && (z2 <= (comms_boundary_altitude_ + comms_boundary_epsilon_));
     return (both_above || both_below);
 }
 
-} // namespace network
-} // namespace scrimmage
+}  // namespace network
+}  // namespace scrimmage
diff --git a/src/plugins/sensor/AirSimSensor/AirSimSensor.cpp b/src/plugins/sensor/AirSimSensor/AirSimSensor.cpp
index 607227e4ff..cbd26f0544 100644
--- a/src/plugins/sensor/AirSimSensor/AirSimSensor.cpp
+++ b/src/plugins/sensor/AirSimSensor/AirSimSensor.cpp
@@ -29,35 +29,32 @@
  *
  */
 
-#include <scrimmage/plugins/sensor/AirSimSensor/AirSimSensor.h>
-
-#include <scrimmage/plugin_manager/RegisterPlugin.h>
+#include <scrimmage/common/Random.h>
+#include <scrimmage/common/Time.h>
 #include <scrimmage/entity/Entity.h>
+#include <scrimmage/math/Angles.h>
+#include <scrimmage/math/Quaternion.h>
 #include <scrimmage/math/State.h>
-#include <scrimmage/parse/ParseUtils.h>
 #include <scrimmage/parse/MissionParse.h>
+#include <scrimmage/parse/ParseUtils.h>
+#include <scrimmage/plugin_manager/RegisterPlugin.h>
+#include <scrimmage/plugins/sensor/AirSimSensor/AirSimSensor.h>
+#include <scrimmage/proto/State.pb.h>
 #include <scrimmage/pubsub/Message.h>
 #include <scrimmage/pubsub/Publisher.h>
-#include <scrimmage/proto/State.pb.h>
-#include <scrimmage/common/Random.h>
-#include <scrimmage/common/Time.h>
-#include <scrimmage/math/Quaternion.h>
-#include <scrimmage/math/Angles.h>
 
+#include <boost/algorithm/string.hpp>  // for boost::split
+#include <boost/filesystem.hpp>
+#include <boost/lexical_cast.hpp>
+#include <chrono>  // NOLINT
 #include <iostream>
-#include <memory>
 #include <limits>
-#include <chrono> // NOLINT
-#include <thread> // NOLINT
-#include <mutex> // NOLINT
-
+#include <memory>
+#include <mutex>  // NOLINT
 #include <opencv2/core/core.hpp>
 #include <opencv2/highgui/highgui.hpp>
 #include <opencv2/imgproc/imgproc.hpp>
-
-#include <boost/lexical_cast.hpp>
-#include <boost/algorithm/string.hpp> // for boost::split
-#include <boost/filesystem.hpp>
+#include <thread>  // NOLINT
 
 #include "common/AirSimSettings.hpp"
 
@@ -74,37 +71,38 @@ REGISTER_PLUGIN(scrimmage::Sensor, scrimmage::sensor::AirSimSensor, AirSimSensor
 namespace scrimmage {
 namespace sensor {
 
-AirSimSensor::AirSimSensor() : client_connected_(false),
-    airsim_ip_("localhost"), airsim_port_(41451), airsim_timeout_s_(60) {
-
+AirSimSensor::AirSimSensor()
+    : client_connected_(false),
+      airsim_ip_("localhost"),
+      airsim_port_(41451),
+      airsim_timeout_s_(60) {
     enu_to_ned_yaw_.set_input_clock_direction(ang::Rotate::CCW);
     enu_to_ned_yaw_.set_input_zero_axis(ang::HeadingZero::Pos_X);
     enu_to_ned_yaw_.set_output_clock_direction(ang::Rotate::CW);
     enu_to_ned_yaw_.set_output_zero_axis(ang::HeadingZero::Pos_Y);
 }
 
-void AirSimSensor::parse_camera_configs(std::map<std::string, std::string> &params) {
+void AirSimSensor::parse_camera_configs(std::map<std::string, std::string>& params) {
     // Parse the camera config string.
     // The string is a list of camera configs from AirSimSensor.xml of the form:
     // [VehicleName=robot1 CameraName=front_center ImageTypeName=Scene Width=256 Height=144]
     std::string camera_config = sc::get<std::string>("camera_config", params, "");
     std::vector<std::string> tokens_1;
     boost::split(tokens_1, camera_config, boost::is_any_of("[]"));
-    for (std::string &t_1 : tokens_1) {
+    for (std::string& t_1 : tokens_1) {
         if (t_1.length() > 0) {
             std::vector<std::string> tokens_2;
             boost::split(tokens_2, t_1, boost::is_any_of(" ,"));
 
             if ((tokens_2.size() == 6) && (tokens_2[0] == vehicle_name_)) {
-
                 try {
                     CameraConfig c;
                     c.vehicle_name = tokens_2[0];
                     c.cam_name = tokens_2[1];
 
                     // Get Image Type Name and Number
-                    // Scene=0, DepthPlanner=1, DepthPerspective=2, DepthVis=3, DisparityNormalized=4,
-                    // Segmentation=5, SurfaceNormals=6, Infrared=7
+                    // Scene=0, DepthPlanner=1, DepthPerspective=2, DepthVis=3,
+                    // DisparityNormalized=4, Segmentation=5, SurfaceNormals=6, Infrared=7
                     if (tokens_2[2] == "Scene") {
                         c.img_type_number = 0;
                         c.img_type = ma::ImageCaptureBase::ImageType::Scene;
@@ -148,25 +146,26 @@ void AirSimSensor::parse_camera_configs(std::map<std::string, std::string> &para
                     c.height = std::stoi(tokens_2[4]);
                     c.fov = std::stoi(tokens_2[5]);
 
-                    cout << "[AirSimSensor] Adding camera to Vehicle '" << vehicle_name_ << "' " << c << endl;
+                    cout << "[AirSimSensor] Adding camera to Vehicle '" << vehicle_name_ << "' "
+                         << c << endl;
                     cam_configs_.push_back(c);
                 } catch (boost::bad_lexical_cast) {
                     // Parsing whitespace and possibily malformed XML.
                     cout << "Parse Error: Check camera_config's in AirSimSensor.xml" << endl;
-                } // end try, catch
+                }  // end try, catch
             }
-        } // end check token_1 size
-    } // end for loop: tokens_2 in token_1
-} // end parse_camera_configs
+        }  // end check token_1 size
+    }  // end for loop: tokens_2 in token_1
+}  // end parse_camera_configs
 
-void AirSimSensor::parse_lidar_configs(std::map<std::string, std::string> &params) {
+void AirSimSensor::parse_lidar_configs(std::map<std::string, std::string>& params) {
     // Parse the lidar config string.
     // The string is a list of lidar configs from AirSimSensor.xml of the form:
     // [VehicleName LidarName]
     std::string lidar_config = sc::get<std::string>("lidar_config", params, "");
     std::vector<std::string> tokens_1;
     boost::split(tokens_1, lidar_config, boost::is_any_of("[]"));
-    for (std::string &t_1 : tokens_1) {
+    for (std::string& t_1 : tokens_1) {
         if (t_1.length() > 0) {
             std::vector<std::string> tokens_2;
             boost::split(tokens_2, t_1, boost::is_any_of(" ,"));
@@ -184,22 +183,22 @@ void AirSimSensor::parse_lidar_configs(std::map<std::string, std::string> &param
     }
 }
 
-void AirSimSensor::parse_imu_configs(std::map<std::string, std::string> &params) {
+void AirSimSensor::parse_imu_configs(std::map<std::string, std::string>& params) {
     // Parse the imu config string.
     // The string is a list of imu configs from AirSimSensor.xml of the form:
     // [VehicleName ImuName]
     std::string imu_config = sc::get<std::string>("imu_config", params, "");
     std::vector<std::string> tokens_1;
     boost::split(tokens_1, imu_config, boost::is_any_of("[]"));
-    for (std::string &t_1 : tokens_1) {
+    for (std::string& t_1 : tokens_1) {
         if (t_1.length() > 0) {
             std::vector<std::string> tokens_2;
             boost::split(tokens_2, t_1, boost::is_any_of(" ,"));
 
             if ((tokens_2.size() == 2) && (tokens_2[0] == vehicle_name_)) {
                 try {
-                    cout << "[AirSimSensor] Adding IMU sensor '" << tokens_2[1]
-                         << "' to Vehicle '" << vehicle_name_ << "'." << endl;
+                    cout << "[AirSimSensor] Adding IMU sensor '" << tokens_2[1] << "' to Vehicle '"
+                         << vehicle_name_ << "'." << endl;
                     imu_names_.push_back(tokens_2[1]);
                 } catch (boost::bad_lexical_cast) {
                     // Parsing whitespace and possibily malformed XML.
@@ -209,7 +208,7 @@ void AirSimSensor::parse_imu_configs(std::map<std::string, std::string> &params)
     }
 }
 
-void AirSimSensor::init(std::map<std::string, std::string> &params) {
+void AirSimSensor::init(std::map<std::string, std::string>& params) {
     // RPC / AirSim.xml file
     airsim_ip_ = sc::get<std::string>("airsim_ip", params, "localhost");
     airsim_port_ = sc::get<int>("airsim_port", params, 41451);
@@ -220,7 +219,9 @@ void AirSimSensor::init(std::map<std::string, std::string> &params) {
     cout << "[AirSimSensor] Vehicle Name: " << vehicle_name_ << endl;
     save_airsim_data_ = sc::get<bool>("save_airsim_data", params, "true");
     if (save_airsim_data_) {
-        cout << "[AirSimSensor] Saving camera images and airsim_data.csv of pose to SCRIMMAGE Logs Directory." << endl;
+        cout << "[AirSimSensor] Saving camera images and airsim_data.csv of pose to SCRIMMAGE Logs "
+                "Directory."
+             << endl;
     }
     get_image_data_ = sc::get<bool>("get_image_data", params, "true");
     get_lidar_data_ = sc::get<bool>("get_lidar_data", params, "true");
@@ -230,17 +231,18 @@ void AirSimSensor::init(std::map<std::string, std::string> &params) {
     lidar_acquisition_period_ = sc::get<double>("lidar_acquisition_period", params, 0.1);
     imu_acquisition_period_ = sc::get<double>("imu_acquisition_period", params, 0.1);
 
-
     // Open airsim_data CSV for append (app) and set column headers
-    std::string csv_filename =
-            parent_->mp()->log_dir() + "/airsim_data_robot" + std::to_string(parent_->id().id()) + ".csv";
-    if (!csv.open_output(csv_filename, std::ios_base::app)) std::cout << "Couldn't create csv file" << endl;
+    std::string csv_filename = parent_->mp()->log_dir() + "/airsim_data_robot"
+                               + std::to_string(parent_->id().id()) + ".csv";
+    if (!csv.open_output(csv_filename, std::ios_base::app))
+        std::cout << "Couldn't create csv file" << endl;
     if (!csv.output_is_open()) cout << "File isn't open. Can't write to CSV" << endl;
     csv.set_column_headers("frame, t, x, y, z, roll, pitch, yaw");
 
     //// Publish Images
     if (get_image_data_) {
-        cout << "[AirSimSensor] Retrieving image data within AirSimSensor::request_images() thread." << endl;
+        cout << "[AirSimSensor] Retrieving image data within AirSimSensor::request_images() thread."
+             << endl;
         cout << "[AirSimSensor] Image Acquisition Period = " << image_acquisition_period_ << endl;
         // Get camera configurations
         AirSimSensor::parse_camera_configs(params);
@@ -252,7 +254,8 @@ void AirSimSensor::init(std::map<std::string, std::string> &params) {
 
     //// Publish Lidar
     if (get_lidar_data_) {
-        cout << "[AirSimSensor] Retrieving LIDAR data within AirSimSensor::request_lidar() thread." << endl;
+        cout << "[AirSimSensor] Retrieving LIDAR data within AirSimSensor::request_lidar() thread."
+             << endl;
         cout << "[AirSimSensor] LIDAR Acquisition Period = " << lidar_acquisition_period_ << endl;
         AirSimSensor::parse_lidar_configs(params);
         lidar_pub_ = advertise("LocalNetwork", "AirSimLidar");
@@ -263,7 +266,8 @@ void AirSimSensor::init(std::map<std::string, std::string> &params) {
 
     //// Publish Imu
     if (get_imu_data_) {
-        cout << "[AirSimSensor] Retrieving IMU data within AirSimSensor::request_imu() thread." << endl;
+        cout << "[AirSimSensor] Retrieving IMU data within AirSimSensor::request_imu() thread."
+             << endl;
         cout << "[AirSimSensor] IMU Acquisition Period = " << imu_acquisition_period_ << endl;
         AirSimSensor::parse_imu_configs(params);
         imu_pub_ = advertise("LocalNetwork", "AirSimImu");
@@ -275,9 +279,7 @@ void AirSimSensor::init(std::map<std::string, std::string> &params) {
 
 void AirSimSensor::request_images() {
     std::shared_ptr<ma::RpcLibClientBase> img_client =
-        std::make_shared<ma::MultirotorRpcLibClient>(airsim_ip_,
-                                                     airsim_port_,
-                                                     airsim_timeout_s_);
+        std::make_shared<ma::MultirotorRpcLibClient>(airsim_ip_, airsim_port_, airsim_timeout_s_);
     // cout << vehicle_name_ << " Image client waiting for Unreal/AirSim connection" << endl;
     for (int i = 0; i < 11; i++) {
         if (img_client->getConnectionState() != ma::RpcLibClientBase::ConnectionState::Connected) {
@@ -285,14 +287,16 @@ void AirSimSensor::request_images() {
             std::this_thread::sleep_for(std::chrono::seconds(1));
             // If we haven't been able to connect to AirSim, warn the user and return
             if (i == 10) {
-                cout << "\n[AirSimSensor] Warning: Image client could not connect to AirSim." << endl;
-                cout << "[AirSimSensor] Warning: Please start Unreal/AirSim before the SCRIMMAGE mission.\n" << endl;
+                cout << "\n[AirSimSensor] Warning: Image client could not connect to AirSim."
+                     << endl;
+                cout << "[AirSimSensor] Warning: Please start Unreal/AirSim before the SCRIMMAGE "
+                        "mission.\n"
+                     << endl;
                 return;
             }
             std::shared_ptr<ma::RpcLibClientBase> img_client =
-                    std::make_shared<ma::MultirotorRpcLibClient>(airsim_ip_,
-                                                                 airsim_port_,
-                                                                 airsim_timeout_s_);
+                std::make_shared<ma::MultirotorRpcLibClient>(airsim_ip_, airsim_port_,
+                                                             airsim_timeout_s_);
         } else {
             // cout << vehicle_name_ << " Image Client Connected" << endl;
             break;
@@ -327,14 +331,15 @@ void AirSimSensor::request_images() {
         }
 
         // Get Images
-        const std::vector<ImageResponse>& response_vector  = img_client->simGetImages(requests, vehicle_name_);
+        const std::vector<ImageResponse>& response_vector =
+            img_client->simGetImages(requests, vehicle_name_);
         // request vehicle pose to ensure it is up to date with camera pose
         ma::Pose vehicle_pose_camera = img_client->simGetVehiclePose(vehicle_name_);
         // AirSim vehicle pose is in NED, but scrimmage is in ENU so convert
         // Convert position to ENU: Switch X and Y and negate Z
 
-        // If response vector contains new data, create img_msg, transfer data into message and publish
-        // cout << response_vector.size() << endl;
+        // If response vector contains new data, create img_msg, transfer data into message and
+        // publish cout << response_vector.size() << endl;
         if (response_vector.size() > 0) {
             auto im_msg = std::make_shared<sc::Message<std::vector<AirSimImageType>>>();
             // sc::MessagePtr<std::vector<AirSimImageType>> im_msg;
@@ -355,37 +360,44 @@ void AirSimSensor::request_images() {
                 // AirSim gives pose of vehicle in relation to the world frame in NED
                 Eigen::Translation3f translation_trans_vehicle(vehicle_pose_camera.position);
                 Eigen::Quaternionf rotation_quat_vehicle(vehicle_pose_camera.orientation);
-                Eigen::Isometry3f tf_world_vehicle_NED(translation_trans_vehicle * rotation_quat_vehicle);
+                Eigen::Isometry3f tf_world_vehicle_NED(translation_trans_vehicle
+                                                       * rotation_quat_vehicle);
                 a.vehicle_pose_world_NED = tf_world_vehicle_NED;
                 // AirSim gives pose of camera in relation to the world frame in NED
                 Eigen::Translation3f translation_trans_camera(response.camera_position);
                 Eigen::Quaternionf rotation_quat_camera(response.camera_orientation);
-                Eigen::Isometry3f tf_world_camera_NED(translation_trans_camera * rotation_quat_camera);
+                Eigen::Isometry3f tf_world_camera_NED(translation_trans_camera
+                                                      * rotation_quat_camera);
                 a.camera_pose_world_NED = tf_world_camera_NED;
 
-                // Depth Images (Depth Perspective and Depth Planner) come in as 1 channel float arrays
+                // Depth Images (Depth Perspective and Depth Planner) come in as 1 channel float
+                // arrays
                 a.camera_config.pixels_as_float = response.pixels_as_float;
                 if (a.camera_config.pixels_as_float) {
                     // get uncompressed 32FC1 array bytes
                     a.img = cv::Mat(a.camera_config.height, a.camera_config.width, CV_32FC1);
-                    memcpy(a.img.data, response.image_data_float.data(), response.image_data_float.size() * sizeof(float_t));
+                    memcpy(a.img.data, response.image_data_float.data(),
+                           response.image_data_float.size() * sizeof(float_t));
 
                 } else {
                     // All Other Image types come in as RGB = 3 channel uint8 matrix
                     // However old linux asset environments give 4 channels
 
                     // If image has 4 channels
-                    if ((static_cast<int>(response.image_data_uint8.size())) > (a.camera_config.height * a.camera_config.width * 3)) {
+                    if ((static_cast<int>(response.image_data_uint8.size()))
+                        > (a.camera_config.height * a.camera_config.width * 3)) {
                         a.img = cv::Mat(a.camera_config.height, a.camera_config.width, CV_8UC4);
-                        memcpy(a.img.data, response.image_data_uint8.data(), response.image_data_uint8.size() * sizeof(uint8_t));
+                        memcpy(a.img.data, response.image_data_uint8.data(),
+                               response.image_data_uint8.size() * sizeof(uint8_t));
                     } else {
                         // image has 3 channels
                         a.img = cv::Mat(a.camera_config.height, a.camera_config.width, CV_8UC3);
-                        memcpy(a.img.data, response.image_data_uint8.data(), response.image_data_uint8.size() * sizeof(uint8_t));
+                        memcpy(a.img.data, response.image_data_uint8.data(),
+                               response.image_data_uint8.size() * sizeof(uint8_t));
                     }
-                } // end if pixels_as_float
+                }  // end if pixels_as_float
                 im_msg->data.push_back(a);
-            } // end ImageResponse for loop
+            }  // end ImageResponse for loop
 
             // Submit message to be published
             new_image_mutex_.lock();
@@ -395,45 +407,47 @@ void AirSimSensor::request_images() {
             img_msg_mutex_.lock();
             img_msg_ = im_msg;
             img_msg_mutex_.unlock();
-        } // end response > 0
+        }  // end response > 0
 
         running_mutex_.lock();
         running = running_;
         running_mutex_.unlock();
         // std::this_thread::sleep_for(std::chrono::milliseconds(static_cast<int>(data_acquisition_period_*1000)));
         t_end = std::chrono::high_resolution_clock::now();
-        double t_elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(t_end - t_start).count();
-        t_elapsed = (image_acquisition_period_*1000) - t_elapsed;
+        double t_elapsed =
+            std::chrono::duration_cast<std::chrono::milliseconds>(t_end - t_start).count();
+        t_elapsed = (image_acquisition_period_ * 1000) - t_elapsed;
         if (t_elapsed > 0) {
             std::this_thread::sleep_for(std::chrono::milliseconds(static_cast<int>(t_elapsed)));
         }
         t_end = std::chrono::high_resolution_clock::now();
-        // double image_period_ = std::chrono::duration_cast<std::chrono::duration<double>>(t_end - t_start).count();
-        // cout << "avg image period: " << image_period_ << "s" << endl;
-    } // end while running
-} // end AirSimSensor::request_images
+        // double image_period_ = std::chrono::duration_cast<std::chrono::duration<double>>(t_end -
+        // t_start).count(); cout << "avg image period: " << image_period_ << "s" << endl;
+    }  // end while running
+}  // end AirSimSensor::request_images
 
 void AirSimSensor::request_lidar() {
     std::shared_ptr<ma::RpcLibClientBase> lidar_client =
-            std::make_shared<ma::MultirotorRpcLibClient>(airsim_ip_,
-                                                         airsim_port_,
-                                                         airsim_timeout_s_);
+        std::make_shared<ma::MultirotorRpcLibClient>(airsim_ip_, airsim_port_, airsim_timeout_s_);
 
     // cout << vehicle_name_ << " LIDAR client waiting for Unreal/AirSim connection" << endl;
     for (int i = 0; i < 11; i++) {
-        if (lidar_client->getConnectionState() != ma::RpcLibClientBase::ConnectionState::Connected) {
+        if (lidar_client->getConnectionState()
+            != ma::RpcLibClientBase::ConnectionState::Connected) {
             // cout << "X" << std::flush;
             std::this_thread::sleep_for(std::chrono::seconds(1));
             // If we haven't been able to connect to AirSim, warn the user and return
             if (i == 10) {
-                cout << "\n[AirSimSensor] Warning: LIDAR client could not connect to AirSim." << endl;
-                cout << "[AirSimSensor] Warning: Please start Unreal/AirSim before the SCRIMMAGE mission.\n" << endl;
+                cout << "\n[AirSimSensor] Warning: LIDAR client could not connect to AirSim."
+                     << endl;
+                cout << "[AirSimSensor] Warning: Please start Unreal/AirSim before the SCRIMMAGE "
+                        "mission.\n"
+                     << endl;
                 return;
             }
             std::shared_ptr<ma::RpcLibClientBase> lidar_client =
-                    std::make_shared<ma::MultirotorRpcLibClient>(airsim_ip_,
-                                                                 airsim_port_,
-                                                                 airsim_timeout_s_);
+                std::make_shared<ma::MultirotorRpcLibClient>(airsim_ip_, airsim_port_,
+                                                             airsim_timeout_s_);
         } else {
             // cout << vehicle_name_ << " LIDAR Client Connected" << endl;
             break;
@@ -460,7 +474,8 @@ void AirSimSensor::request_lidar() {
             ma::Pose vehicle_pose = lidar_client->simGetVehiclePose(vehicle_name_);
             Eigen::Translation3f translation_trans_vehicle(vehicle_pose.position);
             Eigen::Quaternionf rotation_quat_vehicle(vehicle_pose.orientation);
-            Eigen::Isometry3f tf_world_vehicle_NED(translation_trans_vehicle * rotation_quat_vehicle);
+            Eigen::Isometry3f tf_world_vehicle_NED(translation_trans_vehicle
+                                                   * rotation_quat_vehicle);
             l.vehicle_pose_world_NED = tf_world_vehicle_NED;
             // Get pose of LIDAR
             Eigen::Translation3f translation_trans_lidar(l.lidar_data.pose.position);
@@ -487,22 +502,21 @@ void AirSimSensor::request_lidar() {
         running_mutex_.unlock();
 
         t_end = std::chrono::high_resolution_clock::now();
-        double t_elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(t_end - t_start).count();
-        t_elapsed = (lidar_acquisition_period_*1000) - t_elapsed;
+        double t_elapsed =
+            std::chrono::duration_cast<std::chrono::milliseconds>(t_end - t_start).count();
+        t_elapsed = (lidar_acquisition_period_ * 1000) - t_elapsed;
         if (t_elapsed > 0) {
             std::this_thread::sleep_for(std::chrono::milliseconds(static_cast<int>(t_elapsed)));
         }
         t_end = std::chrono::high_resolution_clock::now();
-        // double lidar_period_ = std::chrono::duration_cast<std::chrono::duration<double>>(t_end - t_start).count();
-        // cout << "avg lidar period: " << lidar_period_ << "s" << endl;
+        // double lidar_period_ = std::chrono::duration_cast<std::chrono::duration<double>>(t_end -
+        // t_start).count(); cout << "avg lidar period: " << lidar_period_ << "s" << endl;
     }
 }
 
 void AirSimSensor::request_imu() {
     std::shared_ptr<ma::RpcLibClientBase> imu_client =
-            std::make_shared<ma::MultirotorRpcLibClient>(airsim_ip_,
-                                                         airsim_port_,
-                                                         airsim_timeout_s_);
+        std::make_shared<ma::MultirotorRpcLibClient>(airsim_ip_, airsim_port_, airsim_timeout_s_);
     // cout << vehicle_name_ << " IMU waiting for Unreal/AirSim connection" << endl;
     for (int i = 0; i < 11; i++) {
         if (imu_client->getConnectionState() != ma::RpcLibClientBase::ConnectionState::Connected) {
@@ -511,13 +525,14 @@ void AirSimSensor::request_imu() {
             // If we haven't been able to connect to AirSim, warn the user and return
             if (i == 10) {
                 cout << "\n[AirSimSensor] Warning: IMU client could not connect to AirSim." << endl;
-                cout << "[AirSimSensor] Warning: Please start Unreal/AirSim before the SCRIMMAGE mission.\n" << endl;
+                cout << "[AirSimSensor] Warning: Please start Unreal/AirSim before the SCRIMMAGE "
+                        "mission.\n"
+                     << endl;
                 return;
             }
             std::shared_ptr<ma::RpcLibClientBase> imu_client =
-                    std::make_shared<ma::MultirotorRpcLibClient>(airsim_ip_,
-                                                                 airsim_port_,
-                                                                 airsim_timeout_s_);
+                std::make_shared<ma::MultirotorRpcLibClient>(airsim_ip_, airsim_port_,
+                                                             airsim_timeout_s_);
         } else {
             // cout << vehicle_name_ << " IMU Client Connected" << endl;
             break;
@@ -544,7 +559,8 @@ void AirSimSensor::request_imu() {
             ma::Pose vehicle_pose = imu_client->simGetVehiclePose(vehicle_name_);
             Eigen::Translation3f translation_trans_vehicle(vehicle_pose.position);
             Eigen::Quaternionf rotation_quat_vehicle(vehicle_pose.orientation);
-            Eigen::Isometry3f tf_world_vehicle_NED(translation_trans_vehicle * rotation_quat_vehicle);
+            Eigen::Isometry3f tf_world_vehicle_NED(translation_trans_vehicle
+                                                   * rotation_quat_vehicle);
             i.vehicle_pose_world_NED = tf_world_vehicle_NED;
             // Get pose of IMU
             // AirLib API does not give IMU position so use position for center of vehicle
@@ -569,14 +585,15 @@ void AirSimSensor::request_imu() {
         running_mutex_.unlock();
 
         t_end = std::chrono::high_resolution_clock::now();
-        double t_elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(t_end - t_start).count();
-        t_elapsed = (imu_acquisition_period_*1000) - t_elapsed;
+        double t_elapsed =
+            std::chrono::duration_cast<std::chrono::milliseconds>(t_end - t_start).count();
+        t_elapsed = (imu_acquisition_period_ * 1000) - t_elapsed;
         if (t_elapsed > 0) {
             std::this_thread::sleep_for(std::chrono::milliseconds(static_cast<int>(t_elapsed)));
         }
         t_end = std::chrono::high_resolution_clock::now();
-        // double imu_period_ = std::chrono::duration_cast<std::chrono::duration<double>>(t_end - t_start).count();
-        // cout << "avg imu period: " << imu_period_ << "s" << endl;
+        // double imu_period_ = std::chrono::duration_cast<std::chrono::duration<double>>(t_end -
+        // t_start).count(); cout << "avg imu period: " << imu_period_ << "s" << endl;
     }
 }
 
@@ -587,28 +604,32 @@ bool AirSimSensor::step() {
 
     if (!client_connected_) {
         cout << vehicle_name_ << " Sim Client waiting for Unreal/AirSim connection - " << endl;
-        sim_client_ = std::make_shared<ma::MultirotorRpcLibClient>(airsim_ip_,
-                                                                   airsim_port_,
+        sim_client_ = std::make_shared<ma::MultirotorRpcLibClient>(airsim_ip_, airsim_port_,
                                                                    airsim_timeout_s_);
 
         // sim_client_->confirmConnection(); -- don't use infinite while loop
         for (int i = 0; i < 11; i++) {
-            if (sim_client_->getConnectionState() != ma::RpcLibClientBase::ConnectionState::Connected) {
+            if (sim_client_->getConnectionState()
+                != ma::RpcLibClientBase::ConnectionState::Connected) {
                 cout << "X" << std::flush;
                 std::this_thread::sleep_for(std::chrono::seconds(1));
                 // If we haven't been able to connect to AirSim, warn the user and return.
                 if (i == 10) {
-                    cout << "\n[AirSimSensor] Warning: Sim client could not connect to AirSim." << endl;
-                    cout << "[AirSimSensor] Warning: Please start Unreal/AirSim before the SCRIMMAGE mission.\n" << endl;
+                    cout << "\n[AirSimSensor] Warning: Sim client could not connect to AirSim."
+                         << endl;
+                    cout << "[AirSimSensor] Warning: Please start Unreal/AirSim before the "
+                            "SCRIMMAGE mission.\n"
+                         << endl;
                     return false;
                 }
                 std::shared_ptr<ma::RpcLibClientBase> imu_client =
-                        std::make_shared<ma::MultirotorRpcLibClient>(airsim_ip_,
-                                                                     airsim_port_,
-                                                                     airsim_timeout_s_);
+                    std::make_shared<ma::MultirotorRpcLibClient>(airsim_ip_, airsim_port_,
+                                                                 airsim_timeout_s_);
             } else {
                 client_connected_ = true;
-                cout << "[AirSimSensor] Sim Client for " << vehicle_name_ << " connected to AirSim: ip " << airsim_ip_ << ", port " << airsim_port_ << endl;
+                cout << "[AirSimSensor] Sim Client for " << vehicle_name_
+                     << " connected to AirSim: ip " << airsim_ip_ << ", port " << airsim_port_
+                     << endl;
                 break;
             }
         }
@@ -621,7 +642,7 @@ bool AirSimSensor::step() {
     ///////////////////////////////////////////////////////////////////////////
 
     // Setup state information for AirSim
-    sc::StatePtr &state = parent_->state_truth();
+    sc::StatePtr& state = parent_->state_truth();
     // convert from ENU to NED frame
     ma::Vector3r pos(state->pos()(1), state->pos()(0), -state->pos()(2));
 
@@ -630,9 +651,8 @@ bool AirSimSensor::step() {
 
     // pitch, roll, yaw
     // note, the negative pitch and yaw are required because of the wsu coordinate frame
-    ma::Quaternionr qd = ma::VectorMath::toQuaternion(-state->quat().pitch(),
-                                                      state->quat().roll(),
-                                                      airsim_yaw_rad);
+    ma::Quaternionr qd =
+        ma::VectorMath::toQuaternion(-state->quat().pitch(), state->quat().roll(), airsim_yaw_rad);
 
     // Send state information to AirSim
     sim_client_->simSetVehiclePose(ma::Pose(pos, qd), true, vehicle_name_);
@@ -640,7 +660,8 @@ bool AirSimSensor::step() {
     // Get the camera images from the other thread
     if (get_image_data_) {
         // sc::MessagePtr<std::vector<AirSimImageType>> im_msg_step;
-        // auto im_msg_step = std::make_shared<sc::Message<std::vector<AirSimImageType>>>(); // NOLINT
+        // auto im_msg_step = std::make_shared<sc::Message<std::vector<AirSimImageType>>>(); //
+        // NOLINT
 
         new_image_mutex_.lock();
         bool new_image = new_image_;
@@ -664,7 +685,8 @@ bool AirSimSensor::step() {
 
     if (get_lidar_data_) {
         // sc::MessagePtr<AirSimLidarType> lidar_msg;
-        // auto lidar_msg_step = std::make_shared<sc::Message<std::vector<AirSimLidarType>>>(); // NOLINT
+        // auto lidar_msg_step = std::make_shared<sc::Message<std::vector<AirSimLidarType>>>(); //
+        // NOLINT
 
         new_lidar_mutex_.lock();
         bool new_lidar = new_lidar_;
@@ -683,7 +705,8 @@ bool AirSimSensor::step() {
     }
 
     if (get_imu_data_) {
-        // auto imu_msg_step = std::make_shared<sc::Message<std::vector<AirSimImuType>>>(); // NOLINT
+        // auto imu_msg_step = std::make_shared<sc::Message<std::vector<AirSimImuType>>>(); //
+        // NOLINT
 
         new_imu_mutex_.lock();
         bool new_imu = new_imu_;
@@ -706,7 +729,8 @@ bool AirSimSensor::step() {
     return true;
 }
 
-bool AirSimSensor::save_data(MessagePtr<std::vector<AirSimImageType>>& im_msg, sc::StatePtr& state, int frame_num) {
+bool AirSimSensor::save_data(MessagePtr<std::vector<AirSimImageType>>& im_msg, sc::StatePtr& state,
+                             int frame_num) {
     // Get timestamp
     double time_now = time_->t();
 
@@ -730,17 +754,17 @@ bool AirSimSensor::save_data(MessagePtr<std::vector<AirSimImageType>>& im_msg, s
 
     // Write the CSV file to the root log directory file name = airsim_data.csv
     if (!csv.output_is_open()) {
-            cout << "[AirSimSensor] File isn't open. Can't append to CSV" << endl;
-            }
-    csv.append(sc::CSV::Pairs{
-    {"frame", frame_num},
-    {"t", time_now},
-    {"x", state->pos()(0)},
-    {"y", state->pos()(1)},
-    {"z", state->pos()(2)},
-    {"roll", state->quat().roll()},
-    {"pitch", state->quat().pitch()},
-    {"yaw", state->quat().yaw()}}, true, true);
+        cout << "[AirSimSensor] File isn't open. Can't append to CSV" << endl;
+    }
+    csv.append(sc::CSV::Pairs{{"frame", frame_num},
+                              {"t", time_now},
+                              {"x", state->pos()(0)},
+                              {"y", state->pos()(1)},
+                              {"z", state->pos()(2)},
+                              {"roll", state->quat().roll()},
+                              {"pitch", state->quat().pitch()},
+                              {"yaw", state->quat().yaw()}},
+               true, true);
 
     return true;
 }
@@ -760,5 +784,5 @@ void AirSimSensor::close(double t) {
     request_imu_thread_.join();
 }
 
-} // namespace sensor
-} // namespace scrimmage
+}  // namespace sensor
+}  // namespace scrimmage
diff --git a/src/plugins/sensor/AltitudeAboveTerrain/AltitudeAboveTerrain.cpp b/src/plugins/sensor/AltitudeAboveTerrain/AltitudeAboveTerrain.cpp
index b6b3572b88..4624839c75 100644
--- a/src/plugins/sensor/AltitudeAboveTerrain/AltitudeAboveTerrain.cpp
+++ b/src/plugins/sensor/AltitudeAboveTerrain/AltitudeAboveTerrain.cpp
@@ -30,31 +30,29 @@
  *
  */
 
-#include <scrimmage/plugins/sensor/AltitudeAboveTerrain/AltitudeAboveTerrain.h>
-#include <scrimmage/plugin_manager/RegisterPlugin.h>
+#include <scrimmage/common/Random.h>
 #include <scrimmage/entity/Entity.h>
+#include <scrimmage/math/Quaternion.h>
 #include <scrimmage/math/State.h>
 #include <scrimmage/parse/ParseUtils.h>
-#include <scrimmage/pubsub/Message.h>
-#include <scrimmage/proto/State.pb.h>
-#include <scrimmage/common/Random.h>
-#include <scrimmage/math/Quaternion.h>
+#include <scrimmage/plugin_manager/RegisterPlugin.h>
+#include <scrimmage/plugins/sensor/AltitudeAboveTerrain/AltitudeAboveTerrain.h>
 #include <scrimmage/proto/Shape.pb.h>
+#include <scrimmage/proto/State.pb.h>
+#include <scrimmage/pubsub/Message.h>
 #include <scrimmage/pubsub/Subscriber.h>
 
+#include <boost/optional.hpp>
 #include <iostream>
 #include <limits>
 
-#include <boost/optional.hpp>
-
 using std::cout;
 using std::endl;
 
 namespace sc = scrimmage;
 namespace sp = scrimmage_proto;
 
-REGISTER_PLUGIN(scrimmage::Sensor,
-                scrimmage::sensor::AltitudeAboveTerrain,
+REGISTER_PLUGIN(scrimmage::Sensor, scrimmage::sensor::AltitudeAboveTerrain,
                 AltitudeAboveTerrain_plugin)
 
 namespace scrimmage {
@@ -63,23 +61,23 @@ namespace sensor {
 AltitudeAboveTerrain::AltitudeAboveTerrain() {
 }
 
-void AltitudeAboveTerrain::init(std::map<std::string, std::string> &params) {
+void AltitudeAboveTerrain::init(std::map<std::string, std::string>& params) {
     // Use the same generator as the parent so that the simulation is
     // completely deterministic with respect to the simulation seed.
     gener_ = parent_->random()->gener();
 
     std::vector<double> vec;
-    if (str2container(sc::get<std::string>("altitude_noise", params, "0.0, 1.0"),
-                    ", ", vec, 2)) {
+    if (str2container(sc::get<std::string>("altitude_noise", params, "0.0, 1.0"), ", ", vec, 2)) {
         noise_ = parent_->random()->make_rng_normal(vec[0], vec[1]);
     } else {
         noise_ = parent_->random()->make_rng_normal(0.0, 1.0);
     }
 
-    pub_true_ = advertise("GlobalNetwork", "/" + std::to_string(parent_->id().id()) + "/TrueAltitudeAboveTerrain");
+    pub_true_ = advertise("GlobalNetwork",
+                          "/" + std::to_string(parent_->id().id()) + "/TrueAltitudeAboveTerrain");
     pub_noise_ = advertise("LocalNetwork", "AltitudeAboveTerrain");
 
-    auto terrain_map_cb = [&] (auto &msg) {
+    auto terrain_map_cb = [&](auto& msg) {
         map_ = std::make_shared<scrimmage::interaction::TerrainMap>(msg->data);
     };
     subscribe<scrimmage_msgs::Terrain>("GlobalNetwork", "Terrain", terrain_map_cb);
@@ -89,8 +87,8 @@ bool AltitudeAboveTerrain::step() {
     // Wait until we have received a valid terrain map
     if (map_ == nullptr) return true;
 
-    boost::optional<double> height = map_->height_at(parent_->state_truth()->pos()(0),
-                                                     parent_->state_truth()->pos()(1));
+    boost::optional<double> height =
+        map_->height_at(parent_->state_truth()->pos()(0), parent_->state_truth()->pos()(1));
     if (height) {
         double alt = parent_->state_truth()->pos()(2) - *height;
 
@@ -108,5 +106,5 @@ bool AltitudeAboveTerrain::step() {
     }
     return true;
 }
-} // namespace sensor
-} // namespace scrimmage
+}  // namespace sensor
+}  // namespace scrimmage
diff --git a/src/plugins/sensor/ContactBlobCamera/ContactBlobCamera.cpp b/src/plugins/sensor/ContactBlobCamera/ContactBlobCamera.cpp
index 57b0b6b651..081b74411d 100644
--- a/src/plugins/sensor/ContactBlobCamera/ContactBlobCamera.cpp
+++ b/src/plugins/sensor/ContactBlobCamera/ContactBlobCamera.cpp
@@ -30,20 +30,20 @@
  *
  */
 
-#include <scrimmage/plugin_manager/RegisterPlugin.h>
-#include <scrimmage/plugins/sensor/ContactBlobCamera/ContactBlobCamera.h>
 #include <scrimmage/autonomy/Autonomy.h>
 #include <scrimmage/common/ID.h>
-#include <scrimmage/common/Time.h>
 #include <scrimmage/common/RTree.h>
 #include <scrimmage/common/Random.h>
+#include <scrimmage/common/Time.h>
 #include <scrimmage/common/Utilities.h>
 #include <scrimmage/entity/Entity.h>
 #include <scrimmage/math/Angles.h>
 #include <scrimmage/math/Quaternion.h>
 #include <scrimmage/math/State.h>
-#include <scrimmage/parse/ParseUtils.h>
 #include <scrimmage/parse/MissionParse.h>
+#include <scrimmage/parse/ParseUtils.h>
+#include <scrimmage/plugin_manager/RegisterPlugin.h>
+#include <scrimmage/plugins/sensor/ContactBlobCamera/ContactBlobCamera.h>
 #include <scrimmage/plugins/sensor/ContactBlobCamera/ContactBlobCameraType.h>
 #include <scrimmage/proto/ProtoConversions.h>
 #include <scrimmage/proto/Shape.pb.h>
@@ -51,14 +51,12 @@
 #include <scrimmage/pubsub/Message.h>
 #include <scrimmage/pubsub/Publisher.h>
 
-#include <list>
-#include <utility>
-
 #include <GeographicLib/LocalCartesian.hpp>
-
+#include <list>
 #include <opencv2/core/core.hpp>
 #include <opencv2/highgui/highgui.hpp>
 #include <opencv2/imgproc/imgproc.hpp>
+#include <utility>
 
 namespace sc = scrimmage;
 namespace sp = scrimmage_proto;
@@ -68,10 +66,11 @@ REGISTER_PLUGIN(scrimmage::Sensor, scrimmage::sensor::ContactBlobCamera, Contact
 namespace scrimmage {
 namespace sensor {
 
-void ContactBlobCamera::init(std::map<std::string, std::string> &params) {
+void ContactBlobCamera::init(std::map<std::string, std::string>& params) {
     std::stringstream ss;
     ss << parent_->id().id();
-    parameters_file_.open(parent_->mp()->log_dir() + "/blob_sensor_parameters_" + ss.str() + ".txt");
+    parameters_file_.open(parent_->mp()->log_dir() + "/blob_sensor_parameters_" + ss.str()
+                          + ".txt");
 
     gener_ = parent_->random()->gener();
 
@@ -87,7 +86,7 @@ void ContactBlobCamera::init(std::map<std::string, std::string> &params) {
     std::vector<std::vector<std::string>> vecs;
     if (get_vec_of_vecs(sim_det_str, vecs, ", ")) {
         int i = 0;
-        for (auto &vec : vecs) {
+        for (auto& vec : vecs) {
             // First, assume the description is in XYZ
             int id = std::stoi(vec[1]);
             double radius = std::stod(vec[2]);
@@ -100,12 +99,10 @@ void ContactBlobCamera::init(std::map<std::string, std::string> &params) {
                 parent_->projection()->Forward(x, y, z, x, y, z);
             }
 
-            sc::Quaternion quat(std::stod(vec[6]), std::stod(vec[7]),
-                                std::stod(vec[8]));
+            sc::Quaternion quat(std::stod(vec[6]), std::stod(vec[7]), std::stod(vec[8]));
 
             sc::StatePtr cnt_state = std::make_shared<sc::State>(
-                Eigen::Vector3d(x, y, z), Eigen::Vector3d(0, 0, 0),
-                Eigen::Vector3d(0, 0, 0), quat);
+                Eigen::Vector3d(x, y, z), Eigen::Vector3d(0, 0, 0), Eigen::Vector3d(0, 0, 0), quat);
 
             sc::Contact cnt(sc::ID(id, 0, 0), cnt_state);
             cnt.set_type(sc::Contact::Type::MESH);
@@ -126,10 +123,12 @@ void ContactBlobCamera::init(std::map<std::string, std::string> &params) {
     plugin_params_["focal_length"] = sc::get<double>("focal_length", params, 1.0);
     plugin_params_["fps"] = sc::get<double>("frames_per_second", params, 10);
     plugin_params_["az_thresh"] = sc::Angles::deg2rad(sc::get<double>("azimuth_fov", params, 360));
-    plugin_params_["el_thresh"] = sc::Angles::deg2rad(sc::get<double>("elevation_fov", params, 360));
+    plugin_params_["el_thresh"] =
+        sc::Angles::deg2rad(sc::get<double>("elevation_fov", params, 360));
     plugin_params_["fn_prob"] = sc::get<double>("false_negative_probability", params, 0.1);
     plugin_params_["fp_prob"] = sc::get<double>("false_positive_probability", params, 0.1);
-    plugin_params_["max_false_positives"] = sc::get<int>("max_false_positives_per_frame", params, 10);
+    plugin_params_["max_false_positives"] =
+        sc::get<int>("max_false_positives_per_frame", params, 10);
     plugin_params_["std_dev_w"] = sc::get<int>("std_dev_width", params, 10);
     plugin_params_["std_dev_h"] = sc::get<int>("std_dev_height", params, 10);
     plugin_params_["std_dev_h"] = sc::get<int>("std_dev_height", params, 10);
@@ -137,7 +136,8 @@ void ContactBlobCamera::init(std::map<std::string, std::string> &params) {
     set_plugin_params(plugin_params_);
 
     if (log_detections_) {
-        detections_file_.open(parent_->mp()->log_dir() + "/blob_sensor_detections_" + std::to_string(camera_id_) + ".txt");
+        detections_file_.open(parent_->mp()->log_dir() + "/blob_sensor_detections_"
+                              + std::to_string(camera_id_) + ".txt");
     }
 
     for (int i = 0; i < 3; i++) {
@@ -176,7 +176,9 @@ void ContactBlobCamera::init(std::map<std::string, std::string> &params) {
     /**
      * Allow for updating plugin parameters in real-time.
      */
-    auto params_cb = [&](sc::MessagePtr<std::map<std::string, double>> msg) {set_plugin_params(msg->data);};
+    auto params_cb = [&](sc::MessagePtr<std::map<std::string, double>> msg) {
+        set_plugin_params(msg->data);
+    };
     subscribe<std::map<std::string, double>>("LocalNetwork", "BlobPluginParams", params_cb);
 
     /**
@@ -204,24 +206,32 @@ void ContactBlobCamera::init(std::map<std::string, std::string> &params) {
     }
 }
 
-void ContactBlobCamera::draw_frustum(const std::vector<scrimmage_proto::ShapePtr>& frustum_shapes, double x_rot, double y_rot, double z_rot) {
+void ContactBlobCamera::draw_frustum(const std::vector<scrimmage_proto::ShapePtr>& frustum_shapes,
+                                     double x_rot, double y_rot, double z_rot) {
     if (frustum_shapes.size() != 8) {
-        std::cerr << "ContactBlobCamera::draw_frustum: ERROR: input shape vector must be size 8" << std::endl;
+        std::cerr << "ContactBlobCamera::draw_frustum: ERROR: input shape vector must be size 8"
+                  << std::endl;
         return;
     }
     double sensor_footprint_height = max_detect_range_ * tan(el_thresh_ / 2) * 2;
     double sensor_footprint_width = max_detect_range_ * tan(az_thresh_ / 2) * 2;
-    Eigen::Vector3d sensor_UL(max_detect_range_,  sensor_footprint_width / 2, -sensor_footprint_height / 2);
-    Eigen::Vector3d sensor_UR(max_detect_range_,  sensor_footprint_width / 2,  sensor_footprint_height / 2);
-    Eigen::Vector3d sensor_LR(max_detect_range_, -sensor_footprint_width / 2,  sensor_footprint_height / 2);
-    Eigen::Vector3d sensor_LL(max_detect_range_, -sensor_footprint_width / 2, -sensor_footprint_height / 2);
-    std::vector<Eigen::Vector3d> sensor_scene_bb = {sensor_UL, sensor_UR, sensor_LR, sensor_LL, sensor_UL};
+    Eigen::Vector3d sensor_UL(max_detect_range_, sensor_footprint_width / 2,
+                              -sensor_footprint_height / 2);
+    Eigen::Vector3d sensor_UR(max_detect_range_, sensor_footprint_width / 2,
+                              sensor_footprint_height / 2);
+    Eigen::Vector3d sensor_LR(max_detect_range_, -sensor_footprint_width / 2,
+                              sensor_footprint_height / 2);
+    Eigen::Vector3d sensor_LL(max_detect_range_, -sensor_footprint_width / 2,
+                              -sensor_footprint_height / 2);
+    std::vector<Eigen::Vector3d> sensor_scene_bb = {sensor_UL, sensor_UR, sensor_LR, sensor_LL,
+                                                    sensor_UL};
 
     // Rotate into sensor frame
     auto x_rotation = Eigen::AngleAxisd(x_rot, Eigen::Vector3d::UnitX());
     auto y_rotation = Eigen::AngleAxisd(y_rot, Eigen::Vector3d::UnitY());
     auto z_rotation = Eigen::AngleAxisd(z_rot, Eigen::Vector3d::UnitZ());
-    auto A = Eigen::Translation3d(parent_->state_truth()->pos()) * z_rotation * y_rotation * x_rotation;
+    auto A =
+        Eigen::Translation3d(parent_->state_truth()->pos()) * z_rotation * y_rotation * x_rotation;
     for (size_t i = 0; i < sensor_scene_bb.size(); i++) {
         sensor_scene_bb[i] = A * sensor_scene_bb[i];
     }
@@ -247,14 +257,16 @@ void ContactBlobCamera::draw_frustum(const std::vector<scrimmage_proto::ShapePtr
         if (idx_bb == 0) {
             box_edge_start = sensor_scene_bb.back();
         } else {
-            box_edge_start = sensor_scene_bb.at(idx_bb-1);
+            box_edge_start = sensor_scene_bb.at(idx_bb - 1);
         }
         scrimmage::set(frustum_shapes.at(idx_bb)->mutable_line()->mutable_start(), box_edge_start);
         scrimmage::set(frustum_shapes.at(idx_bb)->mutable_line()->mutable_end(), box_edge_end);
 
         // set fov line
-        scrimmage::set(frustum_shapes.at(idx_fov_edge)->mutable_line()->mutable_start(), fov_edge_start);
-        scrimmage::set(frustum_shapes.at(idx_fov_edge)->mutable_line()->mutable_end(), box_edge_end);
+        scrimmage::set(frustum_shapes.at(idx_fov_edge)->mutable_line()->mutable_start(),
+                       fov_edge_start);
+        scrimmage::set(frustum_shapes.at(idx_fov_edge)->mutable_line()->mutable_end(),
+                       box_edge_end);
     }
     // draw all lines
     for (auto&& shape : frustum_shapes) {
@@ -263,18 +275,15 @@ void ContactBlobCamera::draw_frustum(const std::vector<scrimmage_proto::ShapePtr
 }
 
 void ContactBlobCamera::contacts_to_bounding_boxes(
-    const scrimmage::State &sensor_frame,
-    scrimmage::ContactMap &contacts,
-    std::shared_ptr<sc::Message<ContactBlobCameraType>> &msg) {
-
-    for (auto &kv : contacts) {
+    const scrimmage::State& sensor_frame, scrimmage::ContactMap& contacts,
+    std::shared_ptr<sc::Message<ContactBlobCameraType>>& msg) {
+    for (auto& kv : contacts) {
         // Filter out (skip) own contact
         if (kv.second.id().id() == parent_->id().id()) continue;
 
         // Filter out contacts out of range, should use RTree, but rtree still
         // requires querying of ID from contact lists. (TODO)
-        if ((kv.second.state()->pos() - sensor_frame.pos()).norm() >
-            max_detect_range_) {
+        if ((kv.second.state()->pos() - sensor_frame.pos()).norm() > max_detect_range_) {
             continue;
         }
 
@@ -310,13 +319,9 @@ void ContactBlobCamera::contacts_to_bounding_boxes(
         v_r_2 = rot2.toRotationMatrix() * v_r_2;
 
         // Get 3D relative position of rotated vectors
-        Eigen::Vector3d p1(rel_pos(0) + v_r_1(0),
-                           rel_pos(1) + v_r_1(1),
-                           rel_pos(2));
+        Eigen::Vector3d p1(rel_pos(0) + v_r_1(0), rel_pos(1) + v_r_1(1), rel_pos(2));
 
-        Eigen::Vector3d p2(rel_pos(0) + v_r_2(0),
-                           rel_pos(1) + v_r_2(1),
-                           rel_pos(2));
+        Eigen::Vector3d p2(rel_pos(0) + v_r_2(0), rel_pos(1) + v_r_2(1), rel_pos(2));
 
         // Get 2D position of object boundaries
         Eigen::Vector2d r1 = project_rel_3d_to_2d(p1);
@@ -325,22 +330,24 @@ void ContactBlobCamera::contacts_to_bounding_boxes(
         // Calculate image radius using distance between object boundaries
         double object_img_radius = (r1 - r2).norm() / 2.0;
 
-        double object_img_half_width = std::max(1.0, object_img_radius + (*size_noise_[0])(*gener_));
-        double object_img_half_height = std::max(1.0, object_img_radius + (*size_noise_[1])(*gener_));
+        double object_img_half_width =
+            std::max(1.0, object_img_radius + (*size_noise_[0])(*gener_));
+        double object_img_half_height =
+            std::max(1.0, object_img_radius + (*size_noise_[1])(*gener_));
 
         // Add noise to position in 2D image plane
         raster_center(0) += (*pos_noise_[0])(*gener_);
         raster_center(1) += (*pos_noise_[1])(*gener_);
 
         // Add bounding box to frame around object
-        cv::Rect rect(std::floor(raster_center(0))-std::floor(object_img_half_width),
-                      std::floor(raster_center(1))-std::floor(object_img_half_height),
-                      std::floor(object_img_half_width*2) + 1,
-                      std::floor(object_img_half_height*2) + 1);
+        cv::Rect rect(std::floor(raster_center(0)) - std::floor(object_img_half_width),
+                      std::floor(raster_center(1)) - std::floor(object_img_half_height),
+                      std::floor(object_img_half_width * 2) + 1,
+                      std::floor(object_img_half_height * 2) + 1);
 
         if (object_img_radius > 0) {
-            draw_object_with_bounding_box(msg->data.frame, kv.second.id().id(),
-                                          rect, raster_center, object_img_radius);
+            draw_object_with_bounding_box(msg->data.frame, kv.second.id().id(), rect, raster_center,
+                                          object_img_radius);
         }
 
         // Collect all bounding boxes for current detected object
@@ -349,31 +356,28 @@ void ContactBlobCamera::contacts_to_bounding_boxes(
 }
 
 void ContactBlobCamera::add_false_positives(
-    std::shared_ptr<scrimmage::Message<ContactBlobCameraType>> &msg) {
-
+    std::shared_ptr<scrimmage::Message<ContactBlobCameraType>>& msg) {
     for (int i = 0; i < max_false_positives_; i++) {
         double r = parent_->random()->rng_uniform(0.0, 1.0);
         if (r > fp_prob_) continue;
 
         // Generate random center position in frame
-        Eigen::Vector2d raster_center(
-            parent_->random()->rng_uniform_int(0, img_width_),
-            parent_->random()->rng_uniform_int(0, img_height_));
+        Eigen::Vector2d raster_center(parent_->random()->rng_uniform_int(0, img_width_),
+                                      parent_->random()->rng_uniform_int(0, img_height_));
 
         double object_img_radius = parent_->random()->rng_uniform(0.1, 20.0);
 
         // Add bounding box to frame around object
-        cv::Rect rect(std::floor(raster_center(0))-std::floor(object_img_radius),
-                      std::floor(raster_center(1))-std::floor(object_img_radius),
-                      std::floor(object_img_radius*2) + 1,
-                      std::floor(object_img_radius*2) + 1);
+        cv::Rect rect(std::floor(raster_center(0)) - std::floor(object_img_radius),
+                      std::floor(raster_center(1)) - std::floor(object_img_radius),
+                      std::floor(object_img_radius * 2) + 1, std::floor(object_img_radius * 2) + 1);
 
         // Generate a random ID
         int id = parent_->random()->rng_uniform_int(1, 100);
 
         if (object_img_radius > 0) {
-            draw_object_with_bounding_box(msg->data.frame, id, rect,
-                                          raster_center, object_img_radius);
+            draw_object_with_bounding_box(msg->data.frame, id, rect, raster_center,
+                                          object_img_radius);
         }
         msg->data.bounding_boxes[id].push_back(rect);
     }
@@ -384,12 +388,12 @@ bool ContactBlobCamera::step() {
 
     sc::State sensor_frame;
     sensor_frame.quat() =
-            static_cast<sc::Quaternion>(parent_->state_truth()->quat() *
-                                        this->transform()->quat());
+        static_cast<sc::Quaternion>(parent_->state_truth()->quat() * this->transform()->quat());
     sensor_frame.pos() = this->transform()->pos() + parent_->state_truth()->pos();
 
     if (show_frustum_) {
-        draw_frustum(frustum_shapes_, sensor_frame.quat().roll(), sensor_frame.quat().pitch(), sensor_frame.quat().yaw());
+        draw_frustum(frustum_shapes_, sensor_frame.quat().roll(), sensor_frame.quat().pitch(),
+                     sensor_frame.quat().yaw());
     }
 
     auto msg = std::make_shared<sc::Message<ContactBlobCameraType>>();
@@ -419,10 +423,9 @@ bool ContactBlobCamera::step() {
     // Compute bounding boxes for added "simulated" contacts
     contacts_to_bounding_boxes(sensor_frame, sim_contacts_, msg);
 
-    if (show_sim_contacts_ &&
-        time_->t() > last_contact_send_time_ + contact_send_dt_) {
+    if (show_sim_contacts_ && time_->t() > last_contact_send_time_ + contact_send_dt_) {
         // draw_sim_contacts(sim_contacts_);
-        for (auto &kv : sim_contacts_) {
+        for (auto& kv : sim_contacts_) {
             // Draw a sphere at the expected commanded state
             Eigen::Vector3d sphere_center(kv.second.state()->pos().x(),
                                           kv.second.state()->pos().y(),
@@ -446,9 +449,8 @@ bool ContactBlobCamera::step() {
     last_frame_t_ = time_->t();
 
     if (show_image_) {
-        std::string window_name = window_name_ + "-" +
-                std::to_string(parent_->id().id()) + "-" +
-                std::to_string(camera_id_);
+        std::string window_name = window_name_ + "-" + std::to_string(parent_->id().id()) + "-"
+                                  + std::to_string(camera_id_);
         cv::imshow(window_name.c_str(), msg->data.frame);
         cv::waitKey(1);
     }
@@ -475,16 +477,14 @@ Eigen::Vector2d ContactBlobCamera::project_rel_3d_to_2d(Eigen::Vector3d rel_pos)
     Eigen::Vector3d pos(rel_pos(1), rel_pos(2), rel_pos(0));
 
     // Transform to screen / canvas space
-    Eigen::Vector2d s(focal_length_ * -pos(0) / pos(2),
-                      focal_length_ * pos(1) / pos(2));
+    Eigen::Vector2d s(focal_length_ * -pos(0) / pos(2), focal_length_ * pos(1) / pos(2));
 
     // Transform to Normalized Device Coordinate (NDC) space
-    Eigen::Vector2d ndc((s(0) + canvas_width_/2.0) / canvas_width_,
-                        (s(1) + canvas_height_/2.0) / canvas_height_);
+    Eigen::Vector2d ndc((s(0) + canvas_width_ / 2.0) / canvas_width_,
+                        (s(1) + canvas_height_ / 2.0) / canvas_height_);
 
     // Tranform to raster space
-    Eigen::Vector2d r(ndc(0) * img_width_,
-                      (1-ndc(1)) * img_height_);
+    Eigen::Vector2d r(ndc(0) * img_width_, (1 - ndc(1)) * img_height_);
     return r;
 }
 
@@ -492,33 +492,32 @@ bool ContactBlobCamera::in_field_of_view(Eigen::Vector3d rel_pos) {
     double az = atan2(rel_pos(1), rel_pos(0));
     double norm_xy = sqrt(pow(rel_pos(0), 2) + pow(rel_pos(1), 2));
     double el = atan2(rel_pos(2), norm_xy);
-    if (std::abs(az) > (az_thresh_ / 2) ||
-        std::abs(el) > (el_thresh_ / 2)) {
+    if (std::abs(az) > (az_thresh_ / 2) || std::abs(el) > (el_thresh_ / 2)) {
         return false;
     }
     return true;
 }
 
-void ContactBlobCamera::draw_object_with_bounding_box(
-    cv::Mat &frame, const int &id, const cv::Rect &rect,
-    const Eigen::Vector2d &center, const double &radius) {
-
-    Eigen::Vector2i c(std::floor(center(0)),
-                      std::floor(center(1)));
+void ContactBlobCamera::draw_object_with_bounding_box(cv::Mat& frame, const int& id,
+                                                      const cv::Rect& rect,
+                                                      const Eigen::Vector2d& center,
+                                                      const double& radius) {
+    Eigen::Vector2i c(std::floor(center(0)), std::floor(center(1)));
 
-    cv::circle(frame, cv::Point(c(0), c(1)),
-               std::floor(radius), cv::Scalar(255, 255, 255),
-               -1, 8, 0);
+    cv::circle(frame, cv::Point(c(0), c(1)), std::floor(radius), cv::Scalar(255, 255, 255), -1, 8,
+               0);
 
     cv::rectangle(frame, rect, cv::Scalar(0, 0, 255), 1, 8, 0);
 
     cv::Point top_left(rect.x + rect.width, rect.y + rect.height);
-    cv::putText(frame, std::to_string(id), top_left,  cv::FONT_HERSHEY_SIMPLEX,
-                1.0, cv::Scalar(8, 100, 22), 1, 8, false);
+    cv::putText(frame, std::to_string(id), top_left, cv::FONT_HERSHEY_SIMPLEX, 1.0,
+                cv::Scalar(8, 100, 22), 1, 8, false);
 }
 
 void ContactBlobCamera::set_plugin_params(std::map<std::string, double> params) {
-    if (params["senderId"] != parent_->id().id()) { return; }
+    if (params["senderId"] != parent_->id().id()) {
+        return;
+    }
 
     camera_id_ = static_cast<int>(params["camera_id"]);
 
@@ -537,8 +536,12 @@ void ContactBlobCamera::set_plugin_params(std::map<std::string, double> params)
     std_dev_w_ = params["std_dev_w"];
     std_dev_h_ = params["std_dev_h"];
 
-    if (std_dev_w_ > img_width_) { std_dev_w_ = 10; } // TODO: use standard deviation values to inform the false positives
-    if (std_dev_h_ > img_height_) { std_dev_h_ = 10; }
+    if (std_dev_w_ > img_width_) {
+        std_dev_w_ = 10;
+    }  // TODO: use standard deviation values to inform the false positives
+    if (std_dev_h_ > img_height_) {
+        std_dev_h_ = 10;
+    }
 
     canvas_width_ = 2 * focal_length_ * tan(az_thresh_ / 2.0);
     canvas_height_ = 2 * focal_length_ * tan(el_thresh_ / 2.0);
@@ -547,11 +550,11 @@ void ContactBlobCamera::set_plugin_params(std::map<std::string, double> params)
     char buf[100];
     snprintf(buf, sizeof(buf), "\n===================================\n");
     parameters_file_ << buf << std::endl;
-    for (const auto &kv : params) {
+    for (const auto& kv : params) {
         snprintf(buf, sizeof(buf), "%s: %f", kv.first.c_str(), kv.second);
         parameters_file_ << buf << std::endl;
     }
 }
 
-} // namespace sensor
-} // namespace scrimmage
+}  // namespace sensor
+}  // namespace scrimmage
diff --git a/src/plugins/sensor/GPS/GPS.cpp b/src/plugins/sensor/GPS/GPS.cpp
index e60c8bd356..598e2d291a 100644
--- a/src/plugins/sensor/GPS/GPS.cpp
+++ b/src/plugins/sensor/GPS/GPS.cpp
@@ -30,26 +30,24 @@
  *
  */
 
-#include <scrimmage/plugins/sensor/GPS/GPS.h>
-
-#include <scrimmage/plugin_manager/RegisterPlugin.h>
+#include <scrimmage/common/Random.h>
+#include <scrimmage/common/Time.h>
 #include <scrimmage/entity/Entity.h>
 #include <scrimmage/math/State.h>
+#include <scrimmage/msgs/GPS.pb.h>
 #include <scrimmage/parse/ParseUtils.h>
+#include <scrimmage/plugin_manager/RegisterPlugin.h>
+#include <scrimmage/plugins/interaction/Boundary/Boundary.h>
+#include <scrimmage/plugins/interaction/Boundary/BoundaryBase.h>
+#include <scrimmage/plugins/sensor/GPS/GPS.h>
+#include <scrimmage/proto/Shape.pb.h>
+#include <scrimmage/proto/State.pb.h>
 #include <scrimmage/pubsub/Message.h>
 #include <scrimmage/pubsub/Publisher.h>
 #include <scrimmage/pubsub/Subscriber.h>
-#include <scrimmage/proto/State.pb.h>
-#include <scrimmage/common/Random.h>
-#include <scrimmage/common/Time.h>
-#include <scrimmage/proto/Shape.pb.h>
-#include <scrimmage/msgs/GPS.pb.h>
-#include <scrimmage/plugins/interaction/Boundary/BoundaryBase.h>
-#include <scrimmage/plugins/interaction/Boundary/Boundary.h>
 
 #include <GeographicLib/LocalCartesian.hpp>
 
-
 using std::cout;
 using std::endl;
 
@@ -63,17 +61,18 @@ REGISTER_PLUGIN(scrimmage::Sensor, scrimmage::sensor::GPS, GPS_plugin)
 namespace scrimmage {
 namespace sensor {
 
-GPS::GPS(): gps_found_(false), boundary_id_(-1) {}
+GPS::GPS() : gps_found_(false), boundary_id_(-1) {
+}
 
-void GPS::init(std::map<std::string, std::string> &params) {
+void GPS::init(std::map<std::string, std::string>& params) {
     // Loading in params
     if (params.count("gps_denied_ids") > 0) {
-       gps_found_ = str2container(params["gps_denied_ids"], ",", gps_denied_ids_);
+        gps_found_ = str2container(params["gps_denied_ids"], ",", gps_denied_ids_);
     }
 
     pub_ = advertise("GlobalNetwork", "GPSStatus");
 
-    auto bd_cb = [&](auto &msg) {
+    auto bd_cb = [&](auto& msg) {
         boundary_id_ = msg->data.id().id();
         boundary_ = sci::Boundary::make_boundary(msg->data);
     };
@@ -91,8 +90,7 @@ bool GPS::step() {
     double lat = 0;
     double lon = 0;
     double alt = 0;
-    parent_->projection()->Reverse(ns.pos()(0), ns.pos()(1), ns.pos()(2),
-            lat, lon, alt);
+    parent_->projection()->Reverse(ns.pos()(0), ns.pos()(1), ns.pos()(2), lat, lon, alt);
     msg->data.set_lat(lat);
     msg->data.set_lon(lon);
     msg->data.set_alt(alt);
@@ -101,9 +99,10 @@ bool GPS::step() {
 
     bool gps_fix = true;
     if (boundary_ != nullptr && gps_found_) {
-       if (std::find(gps_denied_ids_.begin(), gps_denied_ids_.end(), boundary_id_) !=
-                gps_denied_ids_.end() && boundary_->contains(ns.pos())) {
-                gps_fix = false;
+        if (std::find(gps_denied_ids_.begin(), gps_denied_ids_.end(), boundary_id_)
+                != gps_denied_ids_.end()
+            && boundary_->contains(ns.pos())) {
+            gps_fix = false;
         }
     }
 
@@ -114,5 +113,5 @@ bool GPS::step() {
 
     return true;
 }
-} // namespace sensor
-} // namespace scrimmage
+}  // namespace sensor
+}  // namespace scrimmage
diff --git a/src/plugins/sensor/LOSSensor/LOSSensor.cpp b/src/plugins/sensor/LOSSensor/LOSSensor.cpp
index f0f72da042..3ac7bad7f7 100644
--- a/src/plugins/sensor/LOSSensor/LOSSensor.cpp
+++ b/src/plugins/sensor/LOSSensor/LOSSensor.cpp
@@ -34,16 +34,15 @@
 #include <scrimmage/entity/Entity.h>
 #include <scrimmage/math/Quaternion.h>
 #include <scrimmage/math/State.h>
+#include <scrimmage/msgs/LOSSensor.pb.h>
 #include <scrimmage/parse/ParseUtils.h>
-#include <scrimmage/pubsub/Message.h>
-#include <scrimmage/pubsub/Publisher.h>
 #include <scrimmage/plugin_manager/RegisterPlugin.h>
 #include <scrimmage/plugins/sensor/LOSSensor/LOSSensor.h>
-#include <scrimmage/msgs/LOSSensor.pb.h>
-
-#include <cstdlib>
+#include <scrimmage/pubsub/Message.h>
+#include <scrimmage/pubsub/Publisher.h>
 
 #include <algorithm>
+#include <cstdlib>
 #include <vector>
 
 namespace sc = scrimmage;
@@ -54,26 +53,25 @@ REGISTER_PLUGIN(scrimmage::Sensor, scrimmage::sensor::LOSSensor, LOSSensor_plugi
 namespace scrimmage {
 namespace sensor {
 
-
-LOSSensor::LOSSensor():
-    RayTrace(),
-    sensor_id_(0),
-    last_update_time_(0.0),
-    range_sd_min_(0.0),
-    range_sd_per_unit_(0.0),
-    range_sd_oor_(0.0),
-    oor_return_(0.1),
-    probability_of_error_(0.0),
-    error_sd_(0.0),
-    probability_oor_error_(0.0),
-    use_flat_earth_(true),
-    new_data_(false),
-    range_(0.0),
-    oor_(false),
-    subscribed_(false) {
+LOSSensor::LOSSensor()
+    : RayTrace(),
+      sensor_id_(0),
+      last_update_time_(0.0),
+      range_sd_min_(0.0),
+      range_sd_per_unit_(0.0),
+      range_sd_oor_(0.0),
+      oor_return_(0.1),
+      probability_of_error_(0.0),
+      error_sd_(0.0),
+      probability_oor_error_(0.0),
+      use_flat_earth_(true),
+      new_data_(false),
+      range_(0.0),
+      oor_(false),
+      subscribed_(false) {
 }
 
-void LOSSensor::init(std::map<std::string, std::string> &params) {
+void LOSSensor::init(std::map<std::string, std::string>& params) {
     // Call the super class
     RayTrace::init(params);
 
@@ -113,12 +111,13 @@ bool LOSSensor::step() {
         if (subscribed_ == false) {
             subscribed_ = true;
             // Register for the collision callbacks.  Assume set up for local network publish
-            auto pc_cb = [&] (scrimmage::MessagePtr<sensor::RayTrace::PointCloud> msg) {
+            auto pc_cb = [&](scrimmage::MessagePtr<sensor::RayTrace::PointCloud> msg) {
                 range_ = msg->data.points[0].point.norm();
                 oor_ = msg->data.points[0].oor;
                 new_data_ = true;
             };
-            std::string topic_name = std::to_string(parent_->id().id()) + "/" + name() + "/pointcloud";
+            std::string topic_name =
+                std::to_string(parent_->id().id()) + "/" + name() + "/pointcloud";
             subscribe<sensor::RayTrace::PointCloud>("LocalNetwork", topic_name, pc_cb);
             printf("LOSSensor: Subscribing with topic name %s\n", topic_name.c_str());
         }
@@ -139,8 +138,7 @@ bool LOSSensor::step() {
     double range = 0.0;
     if (use_flat_earth_ == true) {
         // Get appropriate conversion
-        Eigen::Matrix4d tf_m = parent_->state_truth()->tf_matrix(false) *
-                               transform()->tf_matrix();
+        Eigen::Matrix4d tf_m = parent_->state_truth()->tf_matrix(false) * transform()->tf_matrix();
         // Transform sensor's origin to world coordinates
         Eigen::Vector4d sensor_pos = tf_m * Eigen::Vector4d(0, 0, 0, 1);
         Eigen::Vector3d sensor_pos_w = sensor_pos.head<3>() + parent_->state_truth()->pos();
@@ -165,11 +163,11 @@ bool LOSSensor::step() {
 
     if ((oor == false) && (probability_oor_error_ > 0.0)) {
         // Check the random error to see whether it could swap in-range/out-of-range.
-        bool swap_in_out_range =
-            (static_cast<double>(std::rand()) / static_cast<double>(RAND_MAX)) < probability_oor_error_;
+        bool swap_in_out_range = (static_cast<double>(std::rand()) / static_cast<double>(RAND_MAX))
+                                 < probability_oor_error_;
 
-        if ((((range < min_range()) || (range > max_range())) && !swap_in_out_range) ||
-                (((range > min_range()) && (range < max_range())) && swap_in_out_range)) {
+        if ((((range < min_range()) || (range > max_range())) && !swap_in_out_range)
+            || (((range > min_range()) && (range < max_range())) && swap_in_out_range)) {
             // Out of range
             oor = true;
         } else {  // In-range
@@ -185,7 +183,8 @@ bool LOSSensor::step() {
     msg->data.set_sensor_id(sensor_id_);
     if (oor == false) {
         // Add error
-        if ((static_cast<double>(std::rand()) / static_cast<double>(RAND_MAX)) < probability_of_error_) {
+        if ((static_cast<double>(std::rand()) / static_cast<double>(RAND_MAX))
+            < probability_of_error_) {
             // Add additional error
             std::normal_distribution<double> error_dist(0.0, error_sd_);
             double error = error_dist(generator_);
diff --git a/src/plugins/sensor/NoisyContacts/NoisyContacts.cpp b/src/plugins/sensor/NoisyContacts/NoisyContacts.cpp
index 2b59d849ce..32e9ae8bff 100644
--- a/src/plugins/sensor/NoisyContacts/NoisyContacts.cpp
+++ b/src/plugins/sensor/NoisyContacts/NoisyContacts.cpp
@@ -30,19 +30,17 @@
  *
  */
 
-#include <scrimmage/plugins/sensor/NoisyContacts/NoisyContacts.h>
-#include <scrimmage/math/StateWithCovariance.h>
-
-#include <scrimmage/plugin_manager/RegisterPlugin.h>
+#include <scrimmage/common/Random.h>
 #include <scrimmage/entity/Entity.h>
+#include <scrimmage/math/Quaternion.h>
 #include <scrimmage/math/State.h>
+#include <scrimmage/math/StateWithCovariance.h>
 #include <scrimmage/parse/ParseUtils.h>
-
+#include <scrimmage/plugin_manager/RegisterPlugin.h>
+#include <scrimmage/plugins/sensor/NoisyContacts/NoisyContacts.h>
+#include <scrimmage/proto/State.pb.h>
 #include <scrimmage/pubsub/Message.h>
 #include <scrimmage/pubsub/Publisher.h>
-#include <scrimmage/proto/State.pb.h>
-#include <scrimmage/common/Random.h>
-#include <scrimmage/math/Quaternion.h>
 
 #include <iostream>
 #include <limits>
@@ -52,9 +50,7 @@ using std::endl;
 
 namespace sc = scrimmage;
 
-REGISTER_PLUGIN(scrimmage::Sensor,
-                scrimmage::sensor::NoisyContacts,
-                NoisyContacts_plugin)
+REGISTER_PLUGIN(scrimmage::Sensor, scrimmage::sensor::NoisyContacts, NoisyContacts_plugin)
 
 namespace scrimmage {
 namespace sensor {
@@ -62,14 +58,15 @@ namespace sensor {
 NoisyContacts::NoisyContacts() {
 }
 
-void NoisyContacts::init(std::map<std::string, std::string> &params) {
-    auto add_noise = [&](std::string prefix, auto &noise_vec) {
+void NoisyContacts::init(std::map<std::string, std::string>& params) {
+    auto add_noise = [&](std::string prefix, auto& noise_vec) {
         for (int i = 0; i < 3; i++) {
             std::string tag_name = prefix + "_" + std::to_string(i);
             std::vector<double> vec;
             double mean, stdev;
-            std::tie(mean, stdev) = get_vec(tag_name, params, " ", vec, 2) ?
-                std::make_pair(vec[0], vec[1]) : std::make_pair(0.0, 1.0);
+            std::tie(mean, stdev) = get_vec(tag_name, params, " ", vec, 2)
+                                        ? std::make_pair(vec[0], vec[1])
+                                        : std::make_pair(0.0, 1.0);
             noise_vec.push_back(parent_->random()->make_rng_normal(mean, stdev));
         }
     };
@@ -87,7 +84,7 @@ bool NoisyContacts::step() {
     auto msg = std::make_shared<Message<ContactMap>>();
 
     // Create noisy versions of all contacts that aren't own vehicle
-    for (auto &kv: (*parent_->contacts())) {
+    for (auto& kv : (*parent_->contacts())) {
         // ignore own vehicle
         if (kv.second.id().id() == parent_->id().id()) {
             continue;
@@ -96,8 +93,7 @@ bool NoisyContacts::step() {
         // Construct a StateWithCovariance.
         // Covar Matrix is 3x3 (x, y, z) with 5.0 along diagonal
         std::shared_ptr<StateWithCovariance> state =
-            std::make_shared<StateWithCovariance>(*(kv.second.state()), 3, 3,
-                                                  5.0);
+            std::make_shared<StateWithCovariance>(*(kv.second.state()), 3, 3, 5.0);
 
         // Add noise to position and velocity
         for (int i = 0; i < 3; i++) {
@@ -107,13 +103,13 @@ bool NoisyContacts::step() {
 
         // add noise in roll, pitch, yaw order
         state->quat() = state->quat()
-            * Quaternion(Eigen::Vector3d::UnitX(), (*orient_noise_[0])(*gener))
-            * Quaternion(Eigen::Vector3d::UnitY(), (*orient_noise_[1])(*gener))
-            * Quaternion(Eigen::Vector3d::UnitZ(), (*orient_noise_[2])(*gener));
+                        * Quaternion(Eigen::Vector3d::UnitX(), (*orient_noise_[0])(*gener))
+                        * Quaternion(Eigen::Vector3d::UnitY(), (*orient_noise_[1])(*gener))
+                        * Quaternion(Eigen::Vector3d::UnitZ(), (*orient_noise_[2])(*gener));
 
         // Construct a new Contact in place
-        msg->data.emplace(kv.second.id().id(), Contact(kv.second.id(),
-                          std::static_pointer_cast<State>(state)));
+        msg->data.emplace(kv.second.id().id(),
+                          Contact(kv.second.id(), std::static_pointer_cast<State>(state)));
     }
 
     // Publish the noisy contact map
@@ -121,5 +117,5 @@ bool NoisyContacts::step() {
 
     return true;
 }
-} // namespace sensor
-} // namespace scrimmage
+}  // namespace sensor
+}  // namespace scrimmage
diff --git a/src/plugins/sensor/NoisyState/NoisyState.cpp b/src/plugins/sensor/NoisyState/NoisyState.cpp
index f5d68e51a0..87c2116422 100644
--- a/src/plugins/sensor/NoisyState/NoisyState.cpp
+++ b/src/plugins/sensor/NoisyState/NoisyState.cpp
@@ -34,12 +34,12 @@
 #include <scrimmage/entity/Entity.h>
 #include <scrimmage/math/Quaternion.h>
 #include <scrimmage/math/State.h>
+#include <scrimmage/math/StateWithCovariance.h>
 #include <scrimmage/parse/ParseUtils.h>
-#include <scrimmage/pubsub/Message.h>
-#include <scrimmage/pubsub/Publisher.h>
 #include <scrimmage/plugin_manager/RegisterPlugin.h>
 #include <scrimmage/plugins/sensor/NoisyState/NoisyState.h>
-#include <scrimmage/math/StateWithCovariance.h>
+#include <scrimmage/pubsub/Message.h>
+#include <scrimmage/pubsub/Publisher.h>
 
 #include <vector>
 
@@ -50,14 +50,15 @@ REGISTER_PLUGIN(scrimmage::Sensor, scrimmage::sensor::NoisyState, NoisyState_plu
 namespace scrimmage {
 namespace sensor {
 
-void NoisyState::init(std::map<std::string, std::string> &params) {
-    auto add_noise = [&](std::string prefix, auto &noise_vec) {
+void NoisyState::init(std::map<std::string, std::string>& params) {
+    auto add_noise = [&](std::string prefix, auto& noise_vec) {
         for (int i = 0; i < 3; i++) {
             std::string tag_name = prefix + "_" + std::to_string(i);
             std::vector<double> vec;
             double mean, stdev;
-            std::tie(mean, stdev) = get_vec(tag_name, params, " ", vec, 2) ?
-                std::make_pair(vec[0], vec[1]) : std::make_pair(0.0, 1.0);
+            std::tie(mean, stdev) = get_vec(tag_name, params, " ", vec, 2)
+                                        ? std::make_pair(vec[0], vec[1])
+                                        : std::make_pair(0.0, 1.0);
             noise_vec.push_back(parent_->random()->make_rng_normal(mean, stdev));
         }
     };
@@ -91,10 +92,9 @@ bool NoisyState::step() {
 
     // TODO: Test this math.
     // add noise in roll, pitch, yaw order
-    msg->data.quat() = ns.quat()
-        * Quaternion(Eigen::Vector3d::UnitX(), (*orient_noise_[0])(*gener))
-        * Quaternion(Eigen::Vector3d::UnitY(), (*orient_noise_[1])(*gener))
-        * Quaternion(Eigen::Vector3d::UnitZ(), (*orient_noise_[2])(*gener));
+    msg->data.quat() = ns.quat() * Quaternion(Eigen::Vector3d::UnitX(), (*orient_noise_[0])(*gener))
+                       * Quaternion(Eigen::Vector3d::UnitY(), (*orient_noise_[1])(*gener))
+                       * Quaternion(Eigen::Vector3d::UnitZ(), (*orient_noise_[2])(*gener));
 
     pub_->publish(msg);
 
@@ -103,5 +103,5 @@ bool NoisyState::step() {
 
     return true;
 }
-} // namespace sensor
-} // namespace scrimmage
+}  // namespace sensor
+}  // namespace scrimmage
diff --git a/src/plugins/sensor/RLConsensusSensor/RLConsensusSensor.cpp b/src/plugins/sensor/RLConsensusSensor/RLConsensusSensor.cpp
index 84914e551a..06067121fe 100644
--- a/src/plugins/sensor/RLConsensusSensor/RLConsensusSensor.cpp
+++ b/src/plugins/sensor/RLConsensusSensor/RLConsensusSensor.cpp
@@ -30,18 +30,16 @@
  *
  */
 
-#include <scrimmage/plugins/sensor/RLConsensusSensor/RLConsensusSensor.h>
-
 #include <scrimmage/entity/Entity.h>
 #include <scrimmage/math/State.h>
 #include <scrimmage/parse/ParseUtils.h>
 #include <scrimmage/plugin_manager/RegisterPlugin.h>
-
-#include <iostream>
+#include <scrimmage/plugins/sensor/RLConsensusSensor/RLConsensusSensor.h>
 
 #include <boost/range/adaptor/map.hpp>
 #include <boost/range/adaptor/transformed.hpp>
 #include <boost/range/algorithm/copy.hpp>
+#include <iostream>
 
 namespace sc = scrimmage;
 namespace sp = scrimmage_proto;
@@ -53,7 +51,8 @@ REGISTER_PLUGIN(scrimmage::Sensor, scrimmage::sensor::RLConsensusSensor, RLConse
 namespace scrimmage {
 namespace sensor {
 
-RLConsensusSensor::RLConsensusSensor() : ScrimmageOpenAISensor() {}
+RLConsensusSensor::RLConsensusSensor() : ScrimmageOpenAISensor() {
+}
 
 void RLConsensusSensor::set_observation_space() {
     const int num_veh = parent_->contacts()->size();
@@ -61,19 +60,19 @@ void RLConsensusSensor::set_observation_space() {
     observation_space.continuous_extrema.assign(num_veh, std::make_pair(-inf, inf));
 }
 
-void RLConsensusSensor::get_observation(double *data, uint32_t beg_idx, uint32_t end_idx) {
+void RLConsensusSensor::get_observation(double* data, uint32_t beg_idx, uint32_t end_idx) {
     auto c = parent_->contacts();
     if (c->size() != end_idx - beg_idx) {
         std::cout << "RLConsensusSensor::get_observation (end_idx - beg_idx) "
-            << "does not match number of vehicles" << std::endl;
+                  << "does not match number of vehicles" << std::endl;
         return;
     }
 
     auto ids_view = *c | ba::map_keys;
     std::set<int> ids(ids_view.begin(), ids_view.end());
 
-    auto get_x = [&](int id) {return c->at(id).state()->pos()(0);};
+    auto get_x = [&](int id) { return c->at(id).state()->pos()(0); };
     br::copy(ids | ba::transformed(get_x), data);
 }
-} // namespace sensor
-} // namespace scrimmage
+}  // namespace sensor
+}  // namespace scrimmage
diff --git a/src/plugins/sensor/RLSimpleSensor/RLSimpleSensor.cpp b/src/plugins/sensor/RLSimpleSensor/RLSimpleSensor.cpp
index 52346ce30d..758fac5b91 100644
--- a/src/plugins/sensor/RLSimpleSensor/RLSimpleSensor.cpp
+++ b/src/plugins/sensor/RLSimpleSensor/RLSimpleSensor.cpp
@@ -30,19 +30,18 @@
  *
  */
 
-#include <scrimmage/plugins/sensor/RLSimpleSensor/RLSimpleSensor.h>
-
 #include <scrimmage/common/Time.h>
 #include <scrimmage/entity/Entity.h>
 #include <scrimmage/math/State.h>
 #include <scrimmage/plugin_manager/RegisterPlugin.h>
+#include <scrimmage/plugins/sensor/RLSimpleSensor/RLSimpleSensor.h>
 
 REGISTER_PLUGIN(scrimmage::Sensor, scrimmage::sensor::RLSimpleSensor, RLSimpleSensor_plugin)
 
 namespace scrimmage {
 namespace sensor {
 
-void RLSimpleSensor::get_observation(double *data, uint32_t beg_idx, uint32_t /*end_idx*/) {
+void RLSimpleSensor::get_observation(double* data, uint32_t beg_idx, uint32_t /*end_idx*/) {
     data[beg_idx] = parent_->state()->pos()(0);
     data[beg_idx + 1] = time_->t();
 }
@@ -53,5 +52,5 @@ void RLSimpleSensor::set_observation_space() {
     observation_space.continuous_extrema.push_back(std::make_pair(-inf, inf));
 }
 
-} // namespace sensor
-} // namespace scrimmage
+}  // namespace sensor
+}  // namespace scrimmage
diff --git a/src/plugins/sensor/ROSAltimeter/ROSAltimeter.cpp b/src/plugins/sensor/ROSAltimeter/ROSAltimeter.cpp
index aaa5421e48..45e69a8efd 100644
--- a/src/plugins/sensor/ROSAltimeter/ROSAltimeter.cpp
+++ b/src/plugins/sensor/ROSAltimeter/ROSAltimeter.cpp
@@ -29,20 +29,19 @@
  *
  */
 
-#include <scrimmage/plugins/sensor/ROSAltimeter/ROSAltimeter.h>
-
-#include <scrimmage/plugin_manager/RegisterPlugin.h>
+#include <scrimmage/common/Random.h>
+#include <scrimmage/common/Time.h>
 #include <scrimmage/entity/Entity.h>
+#include <scrimmage/math/Angles.h>
 #include <scrimmage/math/State.h>
 #include <scrimmage/parse/ParseUtils.h>
+#include <scrimmage/plugin_manager/RegisterPlugin.h>
+#include <scrimmage/plugins/sensor/ROSAltimeter/ROSAltimeter.h>
+#include <scrimmage/proto/Shape.pb.h>
+#include <scrimmage/proto/State.pb.h>
 #include <scrimmage/pubsub/Message.h>
 #include <scrimmage/pubsub/Publisher.h>
 #include <scrimmage/pubsub/Subscriber.h>
-#include <scrimmage/proto/State.pb.h>
-#include <scrimmage/common/Random.h>
-#include <scrimmage/common/Time.h>
-#include <scrimmage/proto/Shape.pb.h>
-#include <scrimmage/math/Angles.h>
 
 #define meters2feet 3.28084
 #define feet2meters (1.0 / meters2feet)
@@ -57,9 +56,10 @@ REGISTER_PLUGIN(scrimmage::Sensor, scrimmage::sensor::ROSAltimeter, ROSAltimeter
 namespace scrimmage {
 namespace sensor {
 
-ROSAltimeter::ROSAltimeter() {}
+ROSAltimeter::ROSAltimeter() {
+}
 
-void ROSAltimeter::init(std::map<std::string, std::string> &params) {
+void ROSAltimeter::init(std::map<std::string, std::string>& params) {
     // Setup robot namespace
     vehicle_name_ = sc::get<std::string>("vehicle_name", params, "none");
     if (vehicle_name_ == "none") {
@@ -80,30 +80,32 @@ void ROSAltimeter::init(std::map<std::string, std::string> &params) {
     altimeter_pub_ = nh_->advertise<mavros_msgs::Altitude>(ros_namespace_ + "/altimeter", 1);
 
     // Scrimmage is in East North Up (ENU)
-    // For you to get the ROS data in North East Down (NED): switch x and y, and negate z outside scrimmage.
-    // For more information on the MavROS message format view this page:
+    // For you to get the ROS data in North East Down (NED): switch x and y, and negate z outside
+    // scrimmage. For more information on the MavROS message format view this page:
     // https://mavlink.io/en/messages/common.html#ALTITUDE
 
     // Monotonic Altitude should only be set on initialization and not changed.
     // this altitude value = above sea level (meters)
     ////// MavROS website on Monotonic: //////
-    // This altitude measure is initialized on system boot and monotonic (it is never reset, but represents
-    // the local altitude change). The only guarantee on this field is that it will never be reset and is
-    // consistent within a flight. The recommended value for this field is the uncorrected barometric altitude
-    // at boot time. This altitude will also drift and vary between flights.
-    sc::StatePtr &state = parent_->state_truth();
+    // This altitude measure is initialized on system boot and monotonic (it is never reset, but
+    // represents the local altitude change). The only guarantee on this field is that it will never
+    // be reset and is consistent within a flight. The recommended value for this field is the
+    // uncorrected barometric altitude at boot time. This altitude will also drift and vary between
+    // flights.
+    sc::StatePtr& state = parent_->state_truth();
     double lat_init, lon_init, alt_init;
-    parent_->projection()->Reverse(state->pos()(0), state->pos()(1), state->pos()(2), lat_init, lon_init, alt_init);
+    parent_->projection()->Reverse(state->pos()(0), state->pos()(1), state->pos()(2), lat_init,
+                                   lon_init, alt_init);
     monotonic_ = static_cast<float>(alt_init);
 }
 
 bool ROSAltimeter::step() {
     // Obtain current state information
-    sc::StatePtr &state = parent_->state_truth();
+    sc::StatePtr& state = parent_->state_truth();
 
     // Scrimmage is in East North Up (ENU)
-    // For you to get the ROS data in North East Down (NED): switch x and y, and negate z outside scrimmage.
-    // For more information on the MavROS message format view this page:
+    // For you to get the ROS data in North East Down (NED): switch x and y, and negate z outside
+    // scrimmage. For more information on the MavROS message format view this page:
     // https://mavlink.io/en/messages/common.html#ALTITUDE
 
     // Fill Altitude Message
@@ -111,29 +113,32 @@ bool ROSAltimeter::step() {
     // Scrimmage is in cartesian, use Geographic lib to get lat, long, alt for monotonic
     double lat, lon, alt;
     // cartesian(x, y, z) to (lat, long, alt) using Geographic lib
-    parent_->projection()->Reverse(state->pos()(0), state->pos()(1), state->pos()(2), lat, lon, alt);
+    parent_->projection()->Reverse(state->pos()(0), state->pos()(1), state->pos()(2), lat, lon,
+                                   alt);
     alt_msg.monotonic = monotonic_;
-    // If you print here monotonic will look like it's changing, but if you ROSTopic echo it will be a static number.
-    // cout << alt_msg.monotonic << endl;
+    // If you print here monotonic will look like it's changing, but if you ROSTopic echo it will be
+    // a static number. cout << alt_msg.monotonic << endl;
 
     // amsl = above sea level (meters)
-    // get amsl from the same place as monotonic altitude, but this DOES change throughout the simulation
+    // get amsl from the same place as monotonic altitude, but this DOES change throughout the
+    // simulation
     ////// MavROS website on AMSL: //////
-    // This altitude measure is strictly above mean sea level and might be non-monotonic (it might reset on
-    // events like GPS lock or when a new QNH value is set). It should be the altitude to which global altitude
-    // waypoints are compared to. Note that it is *not* the GPS altitude, however, most GPS modules already
-    // output MSL by default and not the WGS84 altitude.
+    // This altitude measure is strictly above mean sea level and might be non-monotonic (it might
+    // reset on events like GPS lock or when a new QNH value is set). It should be the altitude to
+    // which global altitude waypoints are compared to. Note that it is *not* the GPS altitude,
+    // however, most GPS modules already output MSL by default and not the WGS84 altitude.
     alt_msg.amsl = static_cast<float>(alt);
 
     // local = scrimmage z position of state
     ////// MavROS website on Local: //////
-    // This is the local altitude in the local coordinate frame. It is not the altitude above home, but in reference
-    // to the coordinate origin (0, 0, 0). It is up-positive.
+    // This is the local altitude in the local coordinate frame. It is not the altitude above home,
+    // but in reference to the coordinate origin (0, 0, 0). It is up-positive.
     alt_msg.local = state->pos()(2);
 
     // local - monotonic = relative
     ////// MavROS website on Relative: //////
-    // This is the altitude above the home position. It resets on each change of the current home position.
+    // This is the altitude above the home position. It resets on each change of the current home
+    // position.
     alt_msg.relative = alt_msg.local - alt_msg.monotonic;
 
     // alt_terrain = amsl
@@ -144,15 +149,16 @@ bool ROSAltimeter::step() {
 
     // clearance = same as terrain // laser alt in scrimmage one day?
     ////// MavROS website on Clearance: //////
-    // This is not the altitude, but the clear space below the system according to the fused clearance estimate.
-    // It generally should max out at the maximum range of e.g. the laser altimeter. It is generally a moving target.
-    // A negative value indicates no measurement available.
+    // This is not the altitude, but the clear space below the system according to the fused
+    // clearance estimate. It generally should max out at the maximum range of e.g. the laser
+    // altimeter. It is generally a moving target. A negative value indicates no measurement
+    // available.
     alt_msg.bottom_clearance = alt_msg.amsl;
 
     // Header
-    std_msgs::Header header; // empty header
+    std_msgs::Header header;  // empty header
     // TODO: header.frame = ? // No system for ROS Frame ID's yet
-    header.stamp = ros::Time::now(); // time
+    header.stamp = ros::Time::now();  // time
     alt_msg.header = header;
 
     // Publish Altimeter information
@@ -160,5 +166,5 @@ bool ROSAltimeter::step() {
 
     return true;
 }
-} // namespace sensor
-} // namespace scrimmage
+}  // namespace sensor
+}  // namespace scrimmage
diff --git a/src/plugins/sensor/ROSCompass/ROSCompass.cpp b/src/plugins/sensor/ROSCompass/ROSCompass.cpp
index 9b72d25406..3f6e848dfa 100644
--- a/src/plugins/sensor/ROSCompass/ROSCompass.cpp
+++ b/src/plugins/sensor/ROSCompass/ROSCompass.cpp
@@ -29,20 +29,19 @@
  *
  */
 
-#include <scrimmage/plugins/sensor/ROSCompass/ROSCompass.h>
-
-#include <scrimmage/plugin_manager/RegisterPlugin.h>
+#include <scrimmage/common/Random.h>
+#include <scrimmage/common/Time.h>
 #include <scrimmage/entity/Entity.h>
+#include <scrimmage/math/Angles.h>
 #include <scrimmage/math/State.h>
 #include <scrimmage/parse/ParseUtils.h>
+#include <scrimmage/plugin_manager/RegisterPlugin.h>
+#include <scrimmage/plugins/sensor/ROSCompass/ROSCompass.h>
+#include <scrimmage/proto/Shape.pb.h>
+#include <scrimmage/proto/State.pb.h>
 #include <scrimmage/pubsub/Message.h>
 #include <scrimmage/pubsub/Publisher.h>
 #include <scrimmage/pubsub/Subscriber.h>
-#include <scrimmage/proto/State.pb.h>
-#include <scrimmage/common/Random.h>
-#include <scrimmage/common/Time.h>
-#include <scrimmage/proto/Shape.pb.h>
-#include <scrimmage/math/Angles.h>
 
 using std::cout;
 using std::endl;
@@ -54,9 +53,10 @@ REGISTER_PLUGIN(scrimmage::Sensor, scrimmage::sensor::ROSCompass, ROSCompass_plu
 namespace scrimmage {
 namespace sensor {
 
-ROSCompass::ROSCompass() {}
+ROSCompass::ROSCompass() {
+}
 
-void ROSCompass::init(std::map<std::string, std::string> &params) {
+void ROSCompass::init(std::map<std::string, std::string>& params) {
     // Setup robot namespace
     vehicle_name_ = sc::get<std::string>("vehicle_name", params, "none");
     if (vehicle_name_ == "none") {
@@ -79,24 +79,26 @@ void ROSCompass::init(std::map<std::string, std::string> &params) {
 
 bool ROSCompass::step() {
     // Obtain current state information
-    sc::StatePtr &state = parent_->state_truth();
+    sc::StatePtr& state = parent_->state_truth();
 
     // Scrimmage is in ENU so all outputs are in ENU (East North Up).
 
     // Get rotation vector
-    // Rotate Reverse: Get rotation between orientation of the body and ENU orientation of the north pole (0, 1, 0)
+    // Rotate Reverse: Get rotation between orientation of the body and ENU orientation of the north
+    // pole (0, 1, 0)
     Eigen::Vector3d rotation = state->quat().rotate_reverse(Eigen::Vector3d(0.0, 1.0, 0.0));
 
-    // Fill Compass Message. We're using the magnetic field message, but the info is the rotation vector.
+    // Fill Compass Message. We're using the magnetic field message, but the info is the rotation
+    // vector.
     sensor_msgs::MagneticField compass_msg;
     compass_msg.magnetic_field.x = rotation.x();
     compass_msg.magnetic_field.y = rotation.y();
     compass_msg.magnetic_field.z = rotation.z();
 
     // Header
-    std_msgs::Header header; // empty header
+    std_msgs::Header header;  // empty header
     // TODO: header.frame = ? // No system for ROS Frame ID's yet
-    header.stamp = ros::Time::now(); // time
+    header.stamp = ros::Time::now();  // time
     compass_msg.header = header;
 
     // Publish Compass information
@@ -104,5 +106,5 @@ bool ROSCompass::step() {
 
     return true;
 }
-} // namespace sensor
-} // namespace scrimmage
+}  // namespace sensor
+}  // namespace scrimmage
diff --git a/src/plugins/sensor/ROSIMUSensor/HG4930IMUBudget.cpp b/src/plugins/sensor/ROSIMUSensor/HG4930IMUBudget.cpp
index 3c7b70f130..dd5db9e4ab 100644
--- a/src/plugins/sensor/ROSIMUSensor/HG4930IMUBudget.cpp
+++ b/src/plugins/sensor/ROSIMUSensor/HG4930IMUBudget.cpp
@@ -39,74 +39,122 @@ HG4930IMUBudget::HG4930IMUBudget() {
 void HG4930IMUBudget::CalculateParameters() {
     //// Accel. Parameters:
     // Scale factor
-    XAccelAxisScaleFactorPPMStdDev = AccelScaleFactorPPMStdDev; // 1-sigma in units of PPM
-    YAccelAxisScaleFactorPPMStdDev = AccelScaleFactorPPMStdDev; // 1-sigma in units of PPM
-    ZAccelAxisScaleFactorPPMStdDev = AccelScaleFactorPPMStdDev; // 1-sigma in units of PPM
+    XAccelAxisScaleFactorPPMStdDev = AccelScaleFactorPPMStdDev;  // 1-sigma in units of PPM
+    YAccelAxisScaleFactorPPMStdDev = AccelScaleFactorPPMStdDev;  // 1-sigma in units of PPM
+    ZAccelAxisScaleFactorPPMStdDev = AccelScaleFactorPPMStdDev;  // 1-sigma in units of PPM
     // Misalignment
-    AccelMisalignmentPPMStdDev = (1e6) * sin(AccelMisalignmentRadians); // 1-sigma in units of PPM
+    AccelMisalignmentPPMStdDev = (1e6) * sin(AccelMisalignmentRadians);  // 1-sigma in units of PPM
     // Turn-on Bias
-    XAccelAxisTurnOnBiasMPS2StdDev = AccelBiasRepeatabilityG * (9.8); // 1-sigma in units of meters per second (MPS) squared
-    YAccelAxisTurnOnBiasMPS2StdDev = AccelBiasRepeatabilityG * (9.8); // 1-sigma in units of meters per second (MPS) squared
-    ZAccelAxisTurnOnBiasMPS2StdDev = AccelBiasRepeatabilityG * (9.8); // 1-sigma in units of meters per second (MPS) squared
+    XAccelAxisTurnOnBiasMPS2StdDev =
+        AccelBiasRepeatabilityG * (9.8);  // 1-sigma in units of meters per second (MPS) squared
+    YAccelAxisTurnOnBiasMPS2StdDev =
+        AccelBiasRepeatabilityG * (9.8);  // 1-sigma in units of meters per second (MPS) squared
+    ZAccelAxisTurnOnBiasMPS2StdDev =
+        AccelBiasRepeatabilityG * (9.8);  // 1-sigma in units of meters per second (MPS) squared
     // In-run Bias
-    XAccelAxisInRunBiasMPS2StdDev = AccelBiasInstabilityG * (9.8); // 1-sigma in units of meters per second (MPS) squared
-    YAccelAxisInRunBiasMPS2StdDev = AccelBiasInstabilityG * (9.8); // 1-sigma in units of meters per second (MPS) squared
-    ZAccelAxisInRunBiasMPS2StdDev = AccelBiasInstabilityG * (9.8); // 1-sigma in units of meters per second (MPS) squared
+    XAccelAxisInRunBiasMPS2StdDev =
+        AccelBiasInstabilityG * (9.8);  // 1-sigma in units of meters per second (MPS) squared
+    YAccelAxisInRunBiasMPS2StdDev =
+        AccelBiasInstabilityG * (9.8);  // 1-sigma in units of meters per second (MPS) squared
+    ZAccelAxisInRunBiasMPS2StdDev =
+        AccelBiasInstabilityG * (9.8);  // 1-sigma in units of meters per second (MPS) squared
 
-    XAccelAxisInRunBiasTimeConstantSec = AccelBiasTimeConstantSec; // in-run bias time constant in units of seconds
-    YAccelAxisInRunBiasTimeConstantSec = AccelBiasTimeConstantSec; // in-run bias time constant in units of seconds
-    ZAccelAxisInRunBiasTimeConstantSec = AccelBiasTimeConstantSec; // in-run bias time constant in units of seconds
+    XAccelAxisInRunBiasTimeConstantSec =
+        AccelBiasTimeConstantSec;  // in-run bias time constant in units of seconds
+    YAccelAxisInRunBiasTimeConstantSec =
+        AccelBiasTimeConstantSec;  // in-run bias time constant in units of seconds
+    ZAccelAxisInRunBiasTimeConstantSec =
+        AccelBiasTimeConstantSec;  // in-run bias time constant in units of seconds
 
     ////
     //
     // $$E\{ w^{2}_{a} \} = \frac{2 \sigma^{2}_{b_{a,1}}}{\tau_{a}}$$
     //
 
-    XAccelAxisInRunBiasContinuousProcessNoiseVar = (2 * pow(XAccelAxisInRunBiasMPS2StdDev, 2)) / XAccelAxisInRunBiasTimeConstantSec; // variance of continuous-time process noise driving in-run bias
-    YAccelAxisInRunBiasContinuousProcessNoiseVar = (2 * pow(YAccelAxisInRunBiasMPS2StdDev, 2)) / YAccelAxisInRunBiasTimeConstantSec; // variance of continuous-time process noise driving in-run bias
-    ZAccelAxisInRunBiasContinuousProcessNoiseVar = (2 * pow(ZAccelAxisInRunBiasMPS2StdDev, 2)) / ZAccelAxisInRunBiasTimeConstantSec; // variance of continuous-time process noise driving in-run bias
+    XAccelAxisInRunBiasContinuousProcessNoiseVar =
+        (2 * pow(XAccelAxisInRunBiasMPS2StdDev, 2))
+        / XAccelAxisInRunBiasTimeConstantSec;  // variance of continuous-time process noise driving
+                                               // in-run bias
+    YAccelAxisInRunBiasContinuousProcessNoiseVar =
+        (2 * pow(YAccelAxisInRunBiasMPS2StdDev, 2))
+        / YAccelAxisInRunBiasTimeConstantSec;  // variance of continuous-time process noise driving
+                                               // in-run bias
+    ZAccelAxisInRunBiasContinuousProcessNoiseVar =
+        (2 * pow(ZAccelAxisInRunBiasMPS2StdDev, 2))
+        / ZAccelAxisInRunBiasTimeConstantSec;  // variance of continuous-time process noise driving
+                                               // in-run bias
     // Velocity random walk
-    XAccelVRWMPS2PerRtHzStdDev = (9.8 * AccelVRWGPerRtHz); // velocity random walk (VRW) 1-sigma in units of meters per second (MPS) squared
-    YAccelVRWMPS2PerRtHzStdDev = (9.8 * AccelVRWGPerRtHz); // velocity random walk (VRW) 1-sigma in units of meters per second (MPS) squared
-    ZAccelVRWMPS2PerRtHzStdDev = (9.8 * AccelVRWGPerRtHz); // velocity random walk (VRW) 1-sigma in units of meters per second (MPS) squared
+    XAccelVRWMPS2PerRtHzStdDev =
+        (9.8 * AccelVRWGPerRtHz);  // velocity random walk (VRW) 1-sigma in units of meters per
+                                   // second (MPS) squared
+    YAccelVRWMPS2PerRtHzStdDev =
+        (9.8 * AccelVRWGPerRtHz);  // velocity random walk (VRW) 1-sigma in units of meters per
+                                   // second (MPS) squared
+    ZAccelVRWMPS2PerRtHzStdDev =
+        (9.8 * AccelVRWGPerRtHz);  // velocity random walk (VRW) 1-sigma in units of meters per
+                                   // second (MPS) squared
     // Quantization
     AccelDeltaVQuantizationMPS = localAccelDeltaVQuantizationMPS;
 
     //// Gyro Parameters:
     // Scale factor
-    XGyroAxisScaleFactorPPMStdDev = GyroScaleFactorPPMStdDev; // 1-sigma in units of PPM
-    YGyroAxisScaleFactorPPMStdDev = GyroScaleFactorPPMStdDev; // 1-sigma in units of PPM
-    ZGyroAxisScaleFactorPPMStdDev = GyroScaleFactorPPMStdDev; // 1-sigma in units of PPM
+    XGyroAxisScaleFactorPPMStdDev = GyroScaleFactorPPMStdDev;  // 1-sigma in units of PPM
+    YGyroAxisScaleFactorPPMStdDev = GyroScaleFactorPPMStdDev;  // 1-sigma in units of PPM
+    ZGyroAxisScaleFactorPPMStdDev = GyroScaleFactorPPMStdDev;  // 1-sigma in units of PPM
     // Misalignment
-    GyroMisalignmentPPMStdDev = (1e6) * sin(GyroMisalignmentRadians); // 1-sigma in units of PPM
+    GyroMisalignmentPPMStdDev = (1e6) * sin(GyroMisalignmentRadians);  // 1-sigma in units of PPM
     // Turn-on Bias
-    double GyroBiasStdDevRadPerSec = GyroBiasRepeatabilityDegPerHr * (M_PI / 180.0) * (1.0 / 3600.0); // convert to 1-sigma in radians/sec
+    double GyroBiasStdDevRadPerSec = GyroBiasRepeatabilityDegPerHr * (M_PI / 180.0)
+                                     * (1.0 / 3600.0);  // convert to 1-sigma in radians/sec
     // Now load into appropriate variables
-    XGyroAxisTurnOnBiasRadPerSecStdDev = GyroBiasStdDevRadPerSec; // 1-sigma in units of radians/second
-    YGyroAxisTurnOnBiasRadPerSecStdDev = GyroBiasStdDevRadPerSec; // 1-sigma in units of radians/second
-    ZGyroAxisTurnOnBiasRadPerSecStdDev = GyroBiasStdDevRadPerSec; // 1-sigma in units of radians/second
+    XGyroAxisTurnOnBiasRadPerSecStdDev =
+        GyroBiasStdDevRadPerSec;  // 1-sigma in units of radians/second
+    YGyroAxisTurnOnBiasRadPerSecStdDev =
+        GyroBiasStdDevRadPerSec;  // 1-sigma in units of radians/second
+    ZGyroAxisTurnOnBiasRadPerSecStdDev =
+        GyroBiasStdDevRadPerSec;  // 1-sigma in units of radians/second
     // In-run Bias
-    GyroBiasStdDevRadPerSec = GyroBiasInstabilityDegPerHr * (M_PI / 180.0) * (1.0 / 3600.0); // convert to 1-sigma in radians/sec
+    GyroBiasStdDevRadPerSec = GyroBiasInstabilityDegPerHr * (M_PI / 180.0)
+                              * (1.0 / 3600.0);  // convert to 1-sigma in radians/sec
     // Now load into appropriate variables
-    XGyroAxisInRunBiasRadPerSecStdDev = GyroBiasStdDevRadPerSec; // 1-sigma in units of radians/second
-    YGyroAxisInRunBiasRadPerSecStdDev = GyroBiasStdDevRadPerSec; // 1-sigma in units of radians/second
-    ZGyroAxisInRunBiasRadPerSecStdDev = GyroBiasStdDevRadPerSec; // 1-sigma in units of radians/second
+    XGyroAxisInRunBiasRadPerSecStdDev =
+        GyroBiasStdDevRadPerSec;  // 1-sigma in units of radians/second
+    YGyroAxisInRunBiasRadPerSecStdDev =
+        GyroBiasStdDevRadPerSec;  // 1-sigma in units of radians/second
+    ZGyroAxisInRunBiasRadPerSecStdDev =
+        GyroBiasStdDevRadPerSec;  // 1-sigma in units of radians/second
 
-    XGyroAxisInRunBiasContinuousProcessNoiseVar = (2 * pow(XGyroAxisInRunBiasRadPerSecStdDev, 2)) / GyroBiasInstabilityTimeConstantSec; // variance of continuous-time process noise driving in-run bias
-    YGyroAxisInRunBiasContinuousProcessNoiseVar = (2 * pow(YGyroAxisInRunBiasRadPerSecStdDev, 2)) / GyroBiasInstabilityTimeConstantSec; // variance of continuous-time process noise driving in-run bias
-    ZGyroAxisInRunBiasContinuousProcessNoiseVar = (2 * pow(ZGyroAxisInRunBiasRadPerSecStdDev, 2)) / GyroBiasInstabilityTimeConstantSec; // variance of continuous-time process noise driving in-run bias
+    XGyroAxisInRunBiasContinuousProcessNoiseVar =
+        (2 * pow(XGyroAxisInRunBiasRadPerSecStdDev, 2))
+        / GyroBiasInstabilityTimeConstantSec;  // variance of continuous-time process noise driving
+                                               // in-run bias
+    YGyroAxisInRunBiasContinuousProcessNoiseVar =
+        (2 * pow(YGyroAxisInRunBiasRadPerSecStdDev, 2))
+        / GyroBiasInstabilityTimeConstantSec;  // variance of continuous-time process noise driving
+                                               // in-run bias
+    ZGyroAxisInRunBiasContinuousProcessNoiseVar =
+        (2 * pow(ZGyroAxisInRunBiasRadPerSecStdDev, 2))
+        / GyroBiasInstabilityTimeConstantSec;  // variance of continuous-time process noise driving
+                                               // in-run bias
 
     // Now load time constants
-    XGyroAxisInRunBiasTimeConstantSec = GyroBiasInstabilityTimeConstantSec; // in-run bias time constant in units of seconds
-    YGyroAxisInRunBiasTimeConstantSec = GyroBiasInstabilityTimeConstantSec; // in-run bias time constant in units of seconds
-    ZGyroAxisInRunBiasTimeConstantSec = GyroBiasInstabilityTimeConstantSec; // in-run bias time constant in units of seconds
+    XGyroAxisInRunBiasTimeConstantSec =
+        GyroBiasInstabilityTimeConstantSec;  // in-run bias time constant in units of seconds
+    YGyroAxisInRunBiasTimeConstantSec =
+        GyroBiasInstabilityTimeConstantSec;  // in-run bias time constant in units of seconds
+    ZGyroAxisInRunBiasTimeConstantSec =
+        GyroBiasInstabilityTimeConstantSec;  // in-run bias time constant in units of seconds
 
     // Now do conversions for Angle Random Walk
-    double GyroARWStdDevRadPerSecPerRtHz = GyroARWDegPerRtHr * (M_PI / 180.0) * (1.0 / 60.0); // 1-sigma in radians/second
+    double GyroARWStdDevRadPerSecPerRtHz =
+        GyroARWDegPerRtHr * (M_PI / 180.0) * (1.0 / 60.0);  // 1-sigma in radians/second
     // Now load into appropriate variables
-    XGyroARWRadPerSecPerRtHzStdDev = GyroARWStdDevRadPerSecPerRtHz; // 1-sigma in units of radians/sec
-    YGyroARWRadPerSecPerRtHzStdDev = GyroARWStdDevRadPerSecPerRtHz; // 1-sigma in units of radians/sec
-    ZGyroARWRadPerSecPerRtHzStdDev = GyroARWStdDevRadPerSecPerRtHz; // 1-sigma in units of radians/sec
+    XGyroARWRadPerSecPerRtHzStdDev =
+        GyroARWStdDevRadPerSecPerRtHz;  // 1-sigma in units of radians/sec
+    YGyroARWRadPerSecPerRtHzStdDev =
+        GyroARWStdDevRadPerSecPerRtHz;  // 1-sigma in units of radians/sec
+    ZGyroARWRadPerSecPerRtHzStdDev =
+        GyroARWStdDevRadPerSecPerRtHz;  // 1-sigma in units of radians/sec
     // Quantization
     GyroDeltaThetaQuantizationRadians = localGyroDeltaThetaQuantizationRadians;
 }
diff --git a/src/plugins/sensor/ROSIMUSensor/IMUErrorSimulator.cpp b/src/plugins/sensor/ROSIMUSensor/IMUErrorSimulator.cpp
index b85cd04e37..4f70bdd642 100644
--- a/src/plugins/sensor/ROSIMUSensor/IMUErrorSimulator.cpp
+++ b/src/plugins/sensor/ROSIMUSensor/IMUErrorSimulator.cpp
@@ -23,9 +23,10 @@
  * @author David Burke <david.burke@gtri.gatech.edu>
  * @date 28 May 2020
  * @version 0.1.0
- * @brief Class for simulating randomized IMU error 
+ * @brief Class for simulating randomized IMU error
  * @section DESCRIPTION
- * Class for adding realistic error/noise to 'perfect' IMU data generated from position, velocity, orientation data
+ * Class for adding realistic error/noise to 'perfect' IMU data generated from position, velocity,
+ * orientation data
  *
  */
 
@@ -38,7 +39,8 @@ IMUErrorSimulator::IMUErrorSimulator(IMUErrorBudgetTemplate& errorBudget) {
 }
 
 // generate a 3d vector with the passed in random distribution and generator
-Eigen::Vector3d IMUErrorSimulator::RandomVector(std::normal_distribution<double>& distribution, std::mt19937& generator) {
+Eigen::Vector3d IMUErrorSimulator::RandomVector(std::normal_distribution<double>& distribution,
+                                                std::mt19937& generator) {
     Eigen::Vector3d newVec;
     newVec << distribution(generator), distribution(generator), distribution(generator);
     return newVec;
@@ -47,9 +49,12 @@ Eigen::Vector3d IMUErrorSimulator::RandomVector(std::normal_distribution<double>
 void IMUErrorSimulator::PerformInitialRandomDraws(IMUErrorBudgetTemplate& errorBudget) {
     // Accel Error Terms
     // Get random draws for accel scale factor errors:
-    AccelScaleFactorErr.x() = (1e-6) * errorBudget.XAccelAxisScaleFactorPPMStdDev * InitDist(InitRNG);
-    AccelScaleFactorErr.y() = (1e-6) * errorBudget.YAccelAxisScaleFactorPPMStdDev * InitDist(InitRNG);
-    AccelScaleFactorErr.z() = (1e-6) * errorBudget.ZAccelAxisScaleFactorPPMStdDev * InitDist(InitRNG);
+    AccelScaleFactorErr.x() =
+        (1e-6) * errorBudget.XAccelAxisScaleFactorPPMStdDev * InitDist(InitRNG);
+    AccelScaleFactorErr.y() =
+        (1e-6) * errorBudget.YAccelAxisScaleFactorPPMStdDev * InitDist(InitRNG);
+    AccelScaleFactorErr.z() =
+        (1e-6) * errorBudget.ZAccelAxisScaleFactorPPMStdDev * InitDist(InitRNG);
     // Put scale factor errors in the matrix Ma:
     Ma = AccelScaleFactorErr.asDiagonal();
 
@@ -83,16 +88,30 @@ void IMUErrorSimulator::CalculateParametersForFixedDeltaT(IMUErrorBudgetTemplate
         AccelBiasPhi.y() = exp(-NominalDeltaT / errorBudget.YAccelAxisInRunBiasTimeConstantSec);
         AccelBiasPhi.z() = exp(-NominalDeltaT / errorBudget.ZAccelAxisInRunBiasTimeConstantSec);
 
-        AccelBiasInRunStdDev.x() = errorBudget.XAccelAxisInRunBiasMPS2StdDev * sqrt(1 - exp(-2 * NominalDeltaT / errorBudget.XAccelAxisInRunBiasTimeConstantSec));
-        AccelBiasInRunStdDev.y() = errorBudget.YAccelAxisInRunBiasMPS2StdDev * sqrt(1 - exp(-2 * NominalDeltaT / errorBudget.YAccelAxisInRunBiasTimeConstantSec));
-        AccelBiasInRunStdDev.z() = errorBudget.ZAccelAxisInRunBiasMPS2StdDev * sqrt(1 - exp(-2 * NominalDeltaT / errorBudget.ZAccelAxisInRunBiasTimeConstantSec));
+        AccelBiasInRunStdDev.x() =
+            errorBudget.XAccelAxisInRunBiasMPS2StdDev
+            * sqrt(1 - exp(-2 * NominalDeltaT / errorBudget.XAccelAxisInRunBiasTimeConstantSec));
+        AccelBiasInRunStdDev.y() =
+            errorBudget.YAccelAxisInRunBiasMPS2StdDev
+            * sqrt(1 - exp(-2 * NominalDeltaT / errorBudget.YAccelAxisInRunBiasTimeConstantSec));
+        AccelBiasInRunStdDev.z() =
+            errorBudget.ZAccelAxisInRunBiasMPS2StdDev
+            * sqrt(1 - exp(-2 * NominalDeltaT / errorBudget.ZAccelAxisInRunBiasTimeConstantSec));
         // do initial draw for in-run bias
-        //                AccelBiasInRun = diag([errorBudget.XAccelAxisInRunBiasMPS2StdDev;errorBudget.YAccelAxisInRunBiasMPS2StdDev;errorBudget.ZAccelAxisInRunBiasMPS2StdDev])*randn(AccelBiasInRunRandomNumberStreamPtr,3,1);;
-
-        AccelBiasInRun = Eigen::Vector3d(errorBudget.XAccelAxisInRunBiasMPS2StdDev, errorBudget.YAccelAxisInRunBiasMPS2StdDev, errorBudget.ZAccelAxisInRunBiasMPS2StdDev).asDiagonal() * RandomVector(AccelBiasDist, AccelBiasRNG);
+        //                AccelBiasInRun =
+        //                diag([errorBudget.XAccelAxisInRunBiasMPS2StdDev;errorBudget.YAccelAxisInRunBiasMPS2StdDev;errorBudget.ZAccelAxisInRunBiasMPS2StdDev])*randn(AccelBiasInRunRandomNumberStreamPtr,3,1);;
+
+        AccelBiasInRun = Eigen::Vector3d(errorBudget.XAccelAxisInRunBiasMPS2StdDev,
+                                         errorBudget.YAccelAxisInRunBiasMPS2StdDev,
+                                         errorBudget.ZAccelAxisInRunBiasMPS2StdDev)
+                             .asDiagonal()
+                         * RandomVector(AccelBiasDist, AccelBiasRNG);
     }
     // Following is the standard deviation of the velocity random walk noise
-    AccelVRWMSStdDev = sqrt(NominalDeltaT) * Eigen::Vector3d(errorBudget.XAccelVRWMPS2PerRtHzStdDev, errorBudget.YAccelVRWMPS2PerRtHzStdDev, errorBudget.ZAccelVRWMPS2PerRtHzStdDev);
+    AccelVRWMSStdDev = sqrt(NominalDeltaT)
+                       * Eigen::Vector3d(errorBudget.XAccelVRWMPS2PerRtHzStdDev,
+                                         errorBudget.YAccelVRWMPS2PerRtHzStdDev,
+                                         errorBudget.ZAccelVRWMPS2PerRtHzStdDev);
 
     // Gyro Error Terms:
     // The parameters below are for the gyro in-run bias error
@@ -101,69 +120,111 @@ void IMUErrorSimulator::CalculateParametersForFixedDeltaT(IMUErrorBudgetTemplate
         GyroBiasPhi.y() = exp(-NominalDeltaT / errorBudget.YGyroAxisInRunBiasTimeConstantSec);
         GyroBiasPhi.z() = exp(-NominalDeltaT / errorBudget.ZGyroAxisInRunBiasTimeConstantSec);
 
-        GyroBiasInRunStdDev.x() = errorBudget.XGyroAxisInRunBiasRadPerSecStdDev * sqrt(1 - exp(-2 * NominalDeltaT / errorBudget.XGyroAxisInRunBiasTimeConstantSec));
-        GyroBiasInRunStdDev.y() = errorBudget.YGyroAxisInRunBiasRadPerSecStdDev * sqrt(1 - exp(-2 * NominalDeltaT / errorBudget.YGyroAxisInRunBiasTimeConstantSec));
-        GyroBiasInRunStdDev.z() = errorBudget.ZGyroAxisInRunBiasRadPerSecStdDev * sqrt(1 - exp(-2 * NominalDeltaT / errorBudget.ZGyroAxisInRunBiasTimeConstantSec));
+        GyroBiasInRunStdDev.x() =
+            errorBudget.XGyroAxisInRunBiasRadPerSecStdDev
+            * sqrt(1 - exp(-2 * NominalDeltaT / errorBudget.XGyroAxisInRunBiasTimeConstantSec));
+        GyroBiasInRunStdDev.y() =
+            errorBudget.YGyroAxisInRunBiasRadPerSecStdDev
+            * sqrt(1 - exp(-2 * NominalDeltaT / errorBudget.YGyroAxisInRunBiasTimeConstantSec));
+        GyroBiasInRunStdDev.z() =
+            errorBudget.ZGyroAxisInRunBiasRadPerSecStdDev
+            * sqrt(1 - exp(-2 * NominalDeltaT / errorBudget.ZGyroAxisInRunBiasTimeConstantSec));
         // do initial draw for in-run bias
-        //                GyroBiasInRun = diag([errorBudget.XGyroAxisInRunBiasRadPerSecStdDev;errorBudget.YGyroAxisInRunBiasRadPerSecStdDev;errorBudget.ZGyroAxisInRunBiasRadPerSecStdDev])*randn(GyroBiasInRunRandomNumberStreamPtr,3,1);
-        GyroBiasInRun = Eigen::Vector3d(errorBudget.XGyroAxisInRunBiasRadPerSecStdDev, errorBudget.YGyroAxisInRunBiasRadPerSecStdDev, errorBudget.ZGyroAxisInRunBiasRadPerSecStdDev).asDiagonal() * RandomVector(GyroBiasDist, GyroBiasRNG);
+        //                GyroBiasInRun =
+        //                diag([errorBudget.XGyroAxisInRunBiasRadPerSecStdDev;errorBudget.YGyroAxisInRunBiasRadPerSecStdDev;errorBudget.ZGyroAxisInRunBiasRadPerSecStdDev])*randn(GyroBiasInRunRandomNumberStreamPtr,3,1);
+        GyroBiasInRun = Eigen::Vector3d(errorBudget.XGyroAxisInRunBiasRadPerSecStdDev,
+                                        errorBudget.YGyroAxisInRunBiasRadPerSecStdDev,
+                                        errorBudget.ZGyroAxisInRunBiasRadPerSecStdDev)
+                            .asDiagonal()
+                        * RandomVector(GyroBiasDist, GyroBiasRNG);
     }
     // Following is the standard deviation of the angular random walk noise
-    GyroARWRadStdDev = sqrt(NominalDeltaT) * Eigen::Vector3d(errorBudget.XGyroARWRadPerSecPerRtHzStdDev, errorBudget.YGyroARWRadPerSecPerRtHzStdDev, errorBudget.ZGyroARWRadPerSecPerRtHzStdDev);
+    GyroARWRadStdDev = sqrt(NominalDeltaT)
+                       * Eigen::Vector3d(errorBudget.XGyroARWRadPerSecPerRtHzStdDev,
+                                         errorBudget.YGyroARWRadPerSecPerRtHzStdDev,
+                                         errorBudget.ZGyroARWRadPerSecPerRtHzStdDev);
 }
 
-NoisyIMUData IMUErrorSimulator::EachCycle(IMUErrorBudgetTemplate& errorBudget, Eigen::Vector3d InputDeltaVBodyWRTInertialInBody, Eigen::Vector3d InputDeltaThetaBodyWRTInertialInBody) {
-    Eigen::Vector3d quantizedAccelOutput, OutputBodyFrameAccelErrors, quantizedGyroOutput, OutputBodyFrameRateGyroErrors;
+NoisyIMUData IMUErrorSimulator::EachCycle(IMUErrorBudgetTemplate& errorBudget,
+                                          Eigen::Vector3d InputDeltaVBodyWRTInertialInBody,
+                                          Eigen::Vector3d InputDeltaThetaBodyWRTInertialInBody) {
+    Eigen::Vector3d quantizedAccelOutput, OutputBodyFrameAccelErrors, quantizedGyroOutput,
+        OutputBodyFrameRateGyroErrors;
     NoisyIMUData returnValue;
 
     if (!InRunBiasOption) {
         TotalAccelBias = AccelBiasTurnOn + AccelBiasInRun;
-        AccelBiasInRun = AccelBiasPhi.array() * AccelBiasInRun.array() + AccelBiasInRunStdDev.array() * RandomVector(AccelBiasDist, AccelBiasRNG).array();
+        AccelBiasInRun =
+            AccelBiasPhi.array() * AccelBiasInRun.array()
+            + AccelBiasInRunStdDev.array() * RandomVector(AccelBiasDist, AccelBiasRNG).array();
     }
 
     AccelVRWMSDraws = AccelVRWMSStdDev.array() * RandomVector(AccelVRWDist, AccelVRWRNG).array();
 
     // Calculate errors to be added to the accel data
-    OutputBodyFrameAccelErrors = TotalAccelBias + errorBudget.SampleFrequency * AccelVRWMSDraws + errorBudget.SampleFrequency * Ma * InputDeltaVBodyWRTInertialInBody;
+    OutputBodyFrameAccelErrors =
+        TotalAccelBias + errorBudget.SampleFrequency * AccelVRWMSDraws
+        + errorBudget.SampleFrequency * Ma * InputDeltaVBodyWRTInertialInBody;
 
     // Add errors to accel data
-    Eigen::Vector3d OutputNoisyDeltaVBodyWRTInertialInBody = InputDeltaVBodyWRTInertialInBody + NominalDeltaT * OutputBodyFrameAccelErrors;
+    Eigen::Vector3d OutputNoisyDeltaVBodyWRTInertialInBody =
+        InputDeltaVBodyWRTInertialInBody + NominalDeltaT * OutputBodyFrameAccelErrors;
 
     // Take the noisy delta-V's calculated above in double precision
     // and quantize them based on the accel spec.
-    quantizedAccelOutput = errorBudget.AccelDeltaVQuantizationMPS * ((OutputNoisyDeltaVBodyWRTInertialInBody + AccelQuantizationResiduals).array() / errorBudget.AccelDeltaVQuantizationMPS).round();
+    quantizedAccelOutput =
+        errorBudget.AccelDeltaVQuantizationMPS
+        * ((OutputNoisyDeltaVBodyWRTInertialInBody + AccelQuantizationResiduals).array()
+           / errorBudget.AccelDeltaVQuantizationMPS)
+              .round();
     // Calculate residuals from quantization process and later add them to
     // the next accel measurements to be quantized
-    AccelQuantizationResiduals = OutputNoisyDeltaVBodyWRTInertialInBody + AccelQuantizationResiduals - quantizedAccelOutput;
+    AccelQuantizationResiduals =
+        OutputNoisyDeltaVBodyWRTInertialInBody + AccelQuantizationResiduals - quantizedAccelOutput;
 
     // Calculate totality of errors added to accel data:
-    OutputBodyFrameAccelErrors = errorBudget.SampleFrequency * (quantizedAccelOutput - InputDeltaVBodyWRTInertialInBody).array();
+    OutputBodyFrameAccelErrors =
+        errorBudget.SampleFrequency
+        * (quantizedAccelOutput - InputDeltaVBodyWRTInertialInBody).array();
 
     // Gyro
     // Calculate the total gyro bias, i.e. turn-on + in-run
     if (InRunBiasOption == false) {
         TotalGyroBias = GyroBiasTurnOn + GyroBiasInRun;
         // create the next in-run gyro bias error
-        GyroBiasInRun = GyroBiasPhi.array() * GyroBiasInRun.array() + GyroBiasInRunStdDev.array() * RandomVector(GyroBiasDist, GyroBiasRNG).array();
+        GyroBiasInRun =
+            GyroBiasPhi.array() * GyroBiasInRun.array()
+            + GyroBiasInRunStdDev.array() * RandomVector(GyroBiasDist, GyroBiasRNG).array();
     }
     // do random draw for gyro angel random walk:
     GyroARWRadDraws = GyroARWRadStdDev.array() * RandomVector(GyroARWDist, GyroARWRNG).array();
 
     // Calculate errors to be added to the gyro data
-    OutputBodyFrameRateGyroErrors = TotalGyroBias.array() + errorBudget.SampleFrequency * GyroARWRadDraws.array() + errorBudget.SampleFrequency * (Mg * InputDeltaThetaBodyWRTInertialInBody).array();
+    OutputBodyFrameRateGyroErrors =
+        TotalGyroBias.array() + errorBudget.SampleFrequency * GyroARWRadDraws.array()
+        + errorBudget.SampleFrequency * (Mg * InputDeltaThetaBodyWRTInertialInBody).array();
 
     // Add errors to gyro data
-    Eigen::Vector3d OutputNoisyDeltaThetaBodyWRTInertialInBody = InputDeltaThetaBodyWRTInertialInBody.array() + NominalDeltaT * OutputBodyFrameRateGyroErrors.array();
+    Eigen::Vector3d OutputNoisyDeltaThetaBodyWRTInertialInBody =
+        InputDeltaThetaBodyWRTInertialInBody.array()
+        + NominalDeltaT * OutputBodyFrameRateGyroErrors.array();
 
     // Take the noisy delta-theta's calculated above in double precision
     // and quantize them based on the gyro spec.
-    quantizedGyroOutput = errorBudget.GyroDeltaThetaQuantizationRadians * ((OutputNoisyDeltaThetaBodyWRTInertialInBody + GyroQuantizationResiduals).array() / errorBudget.GyroDeltaThetaQuantizationRadians).round();
+    quantizedGyroOutput =
+        errorBudget.GyroDeltaThetaQuantizationRadians
+        * ((OutputNoisyDeltaThetaBodyWRTInertialInBody + GyroQuantizationResiduals).array()
+           / errorBudget.GyroDeltaThetaQuantizationRadians)
+              .round();
     // Calculate residuals from quantization process and later add them to
     // the next gyro measurements to be quantized
-    GyroQuantizationResiduals = OutputNoisyDeltaThetaBodyWRTInertialInBody + GyroQuantizationResiduals - quantizedGyroOutput;
+    GyroQuantizationResiduals = OutputNoisyDeltaThetaBodyWRTInertialInBody
+                                + GyroQuantizationResiduals - quantizedGyroOutput;
 
     // Calculate totality of errors added to gyro data:
-    OutputBodyFrameRateGyroErrors = errorBudget.SampleFrequency * (quantizedGyroOutput - InputDeltaThetaBodyWRTInertialInBody).array();
+    OutputBodyFrameRateGyroErrors =
+        errorBudget.SampleFrequency
+        * (quantizedGyroOutput - InputDeltaThetaBodyWRTInertialInBody).array();
 
     returnValue.noisyDeltaV = quantizedAccelOutput;
     returnValue.noisyDeltaTheta = quantizedGyroOutput;
diff --git a/src/plugins/sensor/ROSIMUSensor/ROSIMUSensor.cpp b/src/plugins/sensor/ROSIMUSensor/ROSIMUSensor.cpp
index 5931dc2495..be40800ae6 100644
--- a/src/plugins/sensor/ROSIMUSensor/ROSIMUSensor.cpp
+++ b/src/plugins/sensor/ROSIMUSensor/ROSIMUSensor.cpp
@@ -29,24 +29,24 @@
  *
  */
 
-#include <scrimmage/plugins/sensor/ROSIMUSensor/ROSIMUSensor.h>
 #include <math.h>
-#include <scrimmage/plugin_manager/RegisterPlugin.h>
+#include <scrimmage/common/Random.h>
+#include <scrimmage/common/Time.h>
 #include <scrimmage/entity/Entity.h>
+#include <scrimmage/math/Angles.h>
 #include <scrimmage/math/State.h>
 #include <scrimmage/parse/ParseUtils.h>
+#include <scrimmage/plugin_manager/RegisterPlugin.h>
+#include <scrimmage/plugins/sensor/ROSIMUSensor/ROSIMUSensor.h>
+#include <scrimmage/proto/Shape.pb.h>
+#include <scrimmage/proto/State.pb.h>
 #include <scrimmage/pubsub/Message.h>
 #include <scrimmage/pubsub/Publisher.h>
 #include <scrimmage/pubsub/Subscriber.h>
-#include <scrimmage/proto/State.pb.h>
-#include <scrimmage/common/Random.h>
-#include <scrimmage/common/Time.h>
-#include <scrimmage/proto/Shape.pb.h>
-#include <scrimmage/math/Angles.h>
-#include <unsupported/Eigen/MatrixFunctions>
 
-#include <iostream>
 #include <fstream>
+#include <iostream>
+#include <unsupported/Eigen/MatrixFunctions>
 
 using std::cout;
 using std::endl;
@@ -59,9 +59,10 @@ REGISTER_PLUGIN(scrimmage::Sensor, scrimmage::sensor::ROSIMUSensor, ROSIMUSensor
 namespace scrimmage {
 namespace sensor {
 
-ROSIMUSensor::ROSIMUSensor() {}
+ROSIMUSensor::ROSIMUSensor() {
+}
 
-void ROSIMUSensor::init(std::map<std::string, std::string> &params) {
+void ROSIMUSensor::init(std::map<std::string, std::string>& params) {
     // Setup robot namespace
     vehicle_name_ = sc::get<std::string>("vehicle_name", params, "none");
     if (vehicle_name_ == "none") {
@@ -72,9 +73,9 @@ void ROSIMUSensor::init(std::map<std::string, std::string> &params) {
     }
 
     if (!ros::isInitialized()) {
-      int argc = 0;
-      // scrimmage handles it's own SIGINT/SIGTERM shutdown in main.cpp
-      ros::init(argc, NULL, "scrimmage", ros::init_options::NoSigintHandler);
+        int argc = 0;
+        // scrimmage handles it's own SIGINT/SIGTERM shutdown in main.cpp
+        ros::init(argc, NULL, "scrimmage", ros::init_options::NoSigintHandler);
     }
     nh_ = std::make_shared<ros::NodeHandle>();
 
@@ -82,16 +83,21 @@ void ROSIMUSensor::init(std::map<std::string, std::string> &params) {
     imu_pub_ = nh_->advertise<sensor_msgs::Imu>(ros_namespace_ + "/imu", 1);
 
     // Open imu_data CSV for append (app) and set column headers
-    std::string csv_filename = parent_->mp()->log_dir() + "/imu_data_robot" + std::to_string(parent_->id().id()) + ".csv";
-    if (!csv.open_output(csv_filename, std::ios_base::app)) std::cout << "Couldn't create csv file" << endl;
+    std::string csv_filename =
+        parent_->mp()->log_dir() + "/imu_data_robot" + std::to_string(parent_->id().id()) + ".csv";
+    if (!csv.open_output(csv_filename, std::ios_base::app))
+        std::cout << "Couldn't create csv file" << endl;
     if (!csv.output_is_open()) cout << "File isn't open. Can't write to CSV" << endl;
 
-    csv.set_column_headers("time, dt, ECEF_POSX, ECEF_POSY, ECEF_POSZ, ECEF_VELX, ECEF_VELY, ECEF_VELZ, bodyToEcef_X, bodyToEcef_Y, bodyToEcef_Z, bodyToEcef_W, dV_X, dV_Y, dV_Z, dTheta_X, dTheta_Y, dTheta_Z, Noisy_dV_X, Noisy_dV_Y, Noisy_dV_Z, Noisy_dTheta_X, Noisy_dTheta_Y, Noisy_dTheta_Z");
+    csv.set_column_headers(
+        "time, dt, ECEF_POSX, ECEF_POSY, ECEF_POSZ, ECEF_VELX, ECEF_VELY, ECEF_VELZ, bodyToEcef_X, "
+        "bodyToEcef_Y, bodyToEcef_Z, bodyToEcef_W, dV_X, dV_Y, dV_Z, dTheta_X, dTheta_Y, dTheta_Z, "
+        "Noisy_dV_X, Noisy_dV_Y, Noisy_dV_Z, Noisy_dTheta_X, Noisy_dTheta_Y, Noisy_dTheta_Z");
 
     prev_time_ = time_->t();
     first_sample_collected = false;
 
-    error_budget.SampleFrequency = 1.0 / time_->dt(); // Make sure this is valid during init
+    error_budget.SampleFrequency = 1.0 / time_->dt();  // Make sure this is valid during init
     error_sim = new IMUErrorSimulator(error_budget);
 }
 
@@ -141,11 +147,14 @@ Eigen::Vector3d ROSIMUSensor::gravity_ned_from_lla(Eigen::Vector3d lla) {
 
     double w = sqrt(u2 + e2 * sBeta2) / v;
     double q = 0.5 * ((1.0 + 3.0 * u2 / e2) * atan2(e, u) - 3.0 * u / e);
-    double q0 = 0.5 * ((1.0 + 3.0 * b2 / e2) * atan2(e, earth_semiminor_axis) - 3.0 * earth_semiminor_axis / e);
+    double q0 =
+        0.5
+        * ((1.0 + 3.0 * b2 / e2) * atan2(e, earth_semiminor_axis) - 3.0 * earth_semiminor_axis / e);
     double qp = 3.0 * (1.0 + u2 / e2) * (1.0 - (u / e) * atan2(e, u)) - 1.0;
 
     // %   //Mass Attraction Only
-    double Gu = -(wgs84_grav / v2 + omega2 * a2 * e * qp * (0.5 * sBeta2 - 1.0 / 6.0) / (q0 * v2)) / w;
+    double Gu =
+        -(wgs84_grav / v2 + omega2 * a2 * e * qp * (0.5 * sBeta2 - 1.0 / 6.0) / (q0 * v2)) / w;
     double Gb = omega2 * sBeta * cBeta * a2 * q / (w * q0 * v);
     double Psi = atan2(z, sqrt(x2 + y2));
     double sPsi = sin(Psi);
@@ -153,8 +162,10 @@ Eigen::Vector3d ROSIMUSensor::gravity_ned_from_lla(Eigen::Vector3d lla) {
     double Alfa = lla(0) - Psi;
     double sAlfa = sin(Alfa);
     double cAlfa = cos(Alfa);
-    double Gr = Gu * (cPsi * cBeta * u / v + sPsi * sBeta) / w + Gb * (sPsi * cBeta * u / v - cPsi * sBeta) / w;
-    double Gp = -Gu * (sPsi * cBeta * u / v - cPsi * sBeta) / w + Gb * (cPsi * cBeta * u / v + sPsi * sBeta) / w;
+    double Gr = Gu * (cPsi * cBeta * u / v + sPsi * sBeta) / w
+                + Gb * (sPsi * cBeta * u / v - cPsi * sBeta) / w;
+    double Gp = -Gu * (sPsi * cBeta * u / v - cPsi * sBeta) / w
+                + Gb * (cPsi * cBeta * u / v + sPsi * sBeta) / w;
 
     g(0) = -Gr * sAlfa + Gp * cAlfa;
     g(1) = 0.0;
@@ -214,17 +225,18 @@ Eigen::Vector3d ROSIMUSensor::inv_skew_sym(Eigen::Matrix3d inputMatrix) {
 
 Eigen::Matrix3d ROSIMUSensor::enu_to_ecef_rotation(double lat, double lon) {
     Eigen::Matrix3d enuToNed;
-    enuToNed << 0, 1, 0,
-                1, 0, 0,
-                0, 0, -1;
+    enuToNed << 0, 1, 0, 1, 0, 0, 0, 0, -1;
     Eigen::Matrix3d rotationMatrix = ecef_to_ned_rotation(lat, lon);
-    // note don't use .transpose(), bad things happen https://eigen.tuxfamily.org/dox/group__TopicAliasing.html
+    // note don't use .transpose(), bad things happen
+    // https://eigen.tuxfamily.org/dox/group__TopicAliasing.html
     rotationMatrix.transposeInPlace();
     rotationMatrix = (rotationMatrix * enuToNed).eval();
     return rotationMatrix;
 }
 
-Eigen::Vector3d ROSIMUSensor::get_delta_theta(Eigen::Quaterniond qBodyToECEFt1Hat, Eigen::Quaterniond qBodyToECEFt2, double inertialDeltaT) {
+Eigen::Vector3d ROSIMUSensor::get_delta_theta(Eigen::Quaterniond qBodyToECEFt1Hat,
+                                              Eigen::Quaterniond qBodyToECEFt2,
+                                              double inertialDeltaT) {
     Eigen::Matrix3d CBodyToECEFt1Hat(qBodyToECEFt1Hat);
     Eigen::Matrix3d CBodyToECEFt2(qBodyToECEFt2);
     Eigen::Matrix3d CBodyt2ToBodyt1 = CBodyToECEFt1Hat.transpose() * CBodyToECEFt2;
@@ -246,12 +258,15 @@ Eigen::Vector3d ROSIMUSensor::get_delta_theta(Eigen::Quaterniond qBodyToECEFt1Ha
 
     Eigen::Vector3d EarthRateVector;
     EarthRateVector << 0, 0, earth_rate;
-    Eigen::Vector3d deltaThetaBodyWRTInertialInBody = DeltaThetaBodyWRTECEFInBody + inertialDeltaT * CBodyToECEFt1Hat.transpose() * EarthRateVector;
+    Eigen::Vector3d deltaThetaBodyWRTInertialInBody =
+        DeltaThetaBodyWRTECEFInBody
+        + inertialDeltaT * CBodyToECEFt1Hat.transpose() * EarthRateVector;
 
     return deltaThetaBodyWRTInertialInBody;
 }
 
-Eigen::Quaterniond ROSIMUSensor::omega_to_q_dot(Eigen::Quaterniond qBToA, Eigen::Vector3d omegaABInB) {
+Eigen::Quaterniond ROSIMUSensor::omega_to_q_dot(Eigen::Quaterniond qBToA,
+                                                Eigen::Vector3d omegaABInB) {
     Eigen::Quaterniond Temp4By1;
     Temp4By1.w() = 0;
     Temp4By1.vec() = omegaABInB;
@@ -260,29 +275,38 @@ Eigen::Quaterniond ROSIMUSensor::omega_to_q_dot(Eigen::Quaterniond qBToA, Eigen:
     return qBToADot;
 }
 
-Eigen::Quaterniond ROSIMUSensor::integrate_quaternion(Eigen::Quaterniond qBToA, Eigen::Vector3d deltaThetasBFrame) {
-    Eigen::Quaterniond QPredictor(qBToA.coeffs() + omega_to_q_dot(qBToA, deltaThetasBFrame).coeffs());
-    Eigen::Quaterniond QCorrector(qBToA.coeffs() + omega_to_q_dot(QPredictor, deltaThetasBFrame).coeffs());
+Eigen::Quaterniond ROSIMUSensor::integrate_quaternion(Eigen::Quaterniond qBToA,
+                                                      Eigen::Vector3d deltaThetasBFrame) {
+    Eigen::Quaterniond QPredictor(qBToA.coeffs()
+                                  + omega_to_q_dot(qBToA, deltaThetasBFrame).coeffs());
+    Eigen::Quaterniond QCorrector(qBToA.coeffs()
+                                  + omega_to_q_dot(QPredictor, deltaThetasBFrame).coeffs());
     // Perform the addition
     Eigen::Quaterniond sum;
-    sum.w() = QPredictor.w() + QCorrector.w();           // Add the scalar portion
-    sum.vec() = QPredictor.vec() + QCorrector.vec();     // Add the vector portion
-    Eigen::Quaterniond qBToAUpdated(0.5 * sum.coeffs()); // (QPredictor + QCorrector);
+    sum.w() = QPredictor.w() + QCorrector.w();            // Add the scalar portion
+    sum.vec() = QPredictor.vec() + QCorrector.vec();      // Add the vector portion
+    Eigen::Quaterniond qBToAUpdated(0.5 * sum.coeffs());  // (QPredictor + QCorrector);
     qBToAUpdated.coeffs() = qBToAUpdated.coeffs() / qBToAUpdated.norm();
     return qBToAUpdated;
 }
 
-Eigen::Quaterniond ROSIMUSensor::propagate_quaternion(Eigen::Quaterniond qBodyToECEFt1Hat, Eigen::Vector3d deltaThetaBodyWRTInertialInBody, double inertialDeltaT) {
+Eigen::Quaterniond ROSIMUSensor::propagate_quaternion(
+    Eigen::Quaterniond qBodyToECEFt1Hat, Eigen::Vector3d deltaThetaBodyWRTInertialInBody,
+    double inertialDeltaT) {
     Eigen::Matrix3d CBodyToECEFt1Hat(qBodyToECEFt1Hat);
 
     Eigen::Vector3d EarthRateVector;
     EarthRateVector << 0, 0, earth_rate;
-    Eigen::Vector3d deltaThetaBodyWRTECEFInBodyHat = deltaThetaBodyWRTInertialInBody - inertialDeltaT * CBodyToECEFt1Hat.transpose() * EarthRateVector;
-    Eigen::Quaterniond qBodyToECEFt2 = integrate_quaternion(qBodyToECEFt1Hat, deltaThetaBodyWRTECEFInBodyHat);
+    Eigen::Vector3d deltaThetaBodyWRTECEFInBodyHat =
+        deltaThetaBodyWRTInertialInBody
+        - inertialDeltaT * CBodyToECEFt1Hat.transpose() * EarthRateVector;
+    Eigen::Quaterniond qBodyToECEFt2 =
+        integrate_quaternion(qBodyToECEFt1Hat, deltaThetaBodyWRTECEFInBodyHat);
     return qBodyToECEFt2;
 }
 
-Eigen::Vector3d ROSIMUSensor::get_deltaV(Eigen::Vector3d posECEF, Eigen::Vector3d velECEF, Eigen::Quaterniond qBodyToECEF, double InertialDeltaT) {
+Eigen::Vector3d ROSIMUSensor::get_deltaV(Eigen::Vector3d posECEF, Eigen::Vector3d velECEF,
+                                         Eigen::Quaterniond qBodyToECEF, double InertialDeltaT) {
     double k1 = -1, k2 = -1.25;
 
     Eigen::Vector3d Acct1 = (velECEF - vel_t1) / InertialDeltaT;
@@ -297,7 +321,9 @@ Eigen::Vector3d ROSIMUSensor::get_deltaV(Eigen::Vector3d posECEF, Eigen::Vector3
     Eigen::Vector3d EarthRateVector;
     EarthRateVector << 0, 0, earth_rate;
     Eigen::Matrix3d EarthRateSkewSym = skew_sym(EarthRateVector);
-    Eigen::Vector3d deltaV = CECEFToBodyTruth * (Acct1 + 2.0 * EarthRateSkewSym * vel_t1 + EarthRateSkewSym * EarthRateSkewSym * pos_ECEF_t1 - GravityECEF);
+    Eigen::Vector3d deltaV = CECEFToBodyTruth
+                             * (Acct1 + 2.0 * EarthRateSkewSym * vel_t1
+                                + EarthRateSkewSym * EarthRateSkewSym * pos_ECEF_t1 - GravityECEF);
     deltaV = deltaV * InertialDeltaT;
 
     Eigen::Matrix3d CBodyToECEFt1Hat(qBody_to_ECEF_hat);
@@ -313,7 +339,8 @@ Eigen::Vector3d ROSIMUSensor::get_deltaV(Eigen::Vector3d posECEF, Eigen::Vector3
     Eigen::Vector3d AAdd = -k1 * PErr - k2 * VErr;
     deltaV = (deltaV + InertialDeltaT * CBodyToECEFt1Hat.transpose() * AAdd);
 
-    Eigen::Vector3d Ahat = CBodyToECEFt1Hat * deltaV / InertialDeltaT - 2 * EarthRateSkewSym * v_hat - EarthRateSkewSym * EarthRateSkewSym * p_hat + GravityECEF;
+    Eigen::Vector3d Ahat = CBodyToECEFt1Hat * deltaV / InertialDeltaT - 2 * EarthRateSkewSym * v_hat
+                           - EarthRateSkewSym * EarthRateSkewSym * p_hat + GravityECEF;
     Eigen::Vector3d tmpVhat = v_hat + (InertialDeltaT)*Ahat;
     p_hat = p_hat + (InertialDeltaT)*v_hat + 0.5 * (InertialDeltaT * InertialDeltaT) * Ahat;
     v_hat = tmpVhat;
@@ -323,7 +350,7 @@ Eigen::Vector3d ROSIMUSensor::get_deltaV(Eigen::Vector3d posECEF, Eigen::Vector3
 
 bool ROSIMUSensor::step() {
     // Obtain current state information
-    sc::StatePtr &state = parent_->state_truth();
+    sc::StatePtr& state = parent_->state_truth();
     double time_now = time_->t();
     double dt = time_now - prev_time_;
     prev_time_ = time_now;
@@ -332,9 +359,9 @@ bool ROSIMUSensor::step() {
     sensor_msgs::Imu imu_msg;
 
     // Header
-    std_msgs::Header header; // empty header
+    std_msgs::Header header;  // empty header
     // TODO: header.frame = ? // No system for ROS Frame ID's yet
-    header.stamp = ros::Time::now(); // time
+    header.stamp = ros::Time::now();  // time
     imu_msg.header = header;
 
     // get the geodetic origin specified in the mission file
@@ -350,26 +377,27 @@ bool ROSIMUSensor::step() {
 
     // convert the current position (ENU) to ECEF
     Eigen::Vector3d ecefPos = state->pos();
-    ecefPos = enuToEcef * ecefPos;  // rotate
-    ecefPos = ecefPos + ecefOrigin; // translate
+    ecefPos = enuToEcef * ecefPos;   // rotate
+    ecefPos = ecefPos + ecefOrigin;  // translate
 
     // convert (ENU) velocity to ECEF
     Eigen::Vector3d ecefVel = state->vel();
-    ecefVel = enuToEcef * ecefVel; // rotate
+    ecefVel = enuToEcef * ecefVel;  // rotate
 
     // calculate the body to ECEF orientation
     Eigen::Quaterniond qbodyToEnu = state->quat();
     Eigen::Quaterniond qbodyToECEF = qEnuToEcef * qbodyToEnu;
 
     // calculate angular velocity and linear acceleration based on this and the previous frames
-    if (!first_sample_collected) { // for the first sample, just store it, we need 2 samples to perform a calculation
+    if (!first_sample_collected) {  // for the first sample, just store it, we need 2 samples to
+                                    // perform a calculation
         pos_ECEF_t1 = ecefPos;
         vel_t1 = ecefVel;
         prev_qBody_to_ECEF = qbodyToECEF;
         p_hat = pos_ECEF_t1;
         v_hat = vel_t1;
         qBody_to_ECEF_hat = qbodyToECEF;
-        first_sample_collected = true; // first sample has been collected
+        first_sample_collected = true;  // first sample has been collected
     } else {
         // calculate deltas
         Eigen::Vector3d deltaV = get_deltaV(ecefPos, ecefVel, qbodyToECEF, dt);
@@ -418,34 +446,33 @@ bool ROSIMUSensor::step() {
         // Write IMU data to CSV
         // Write the CSV file to the root log directory file name = imu_data.csv
         if (!csv.output_is_open()) {
-        cout << "File isn't open. Can't append to CSV" << endl;
+            cout << "File isn't open. Can't append to CSV" << endl;
         }
-        csv.append(sc::CSV::Pairs{
-                    {"time", time_now},
-                    {"dt", dt},
-                    {"ECEF_POSX", ecefPos.x()},
-                    {"ECEF_POSY", ecefPos.y()},
-                    {"ECEF_POSZ", ecefPos.z()},
-                    {"ECEF_VELX", ecefVel.x()},
-                    {"ECEF_VELY", ecefVel.y()},
-                    {"ECEF_VELZ", ecefVel.z()},
-                    {"bodyToEcef_X", qbodyToECEF.x()},
-                    {"bodyToEcef_Y", qbodyToECEF.y()},
-                    {"bodyToEcef_Z", qbodyToECEF.z()},
-                    {"bodyToEcef_W", qbodyToECEF.w()},
-                    {"dV_X", deltaV.x()},
-                    {"dV_Y", deltaV.y()},
-                    {"dV_Z", deltaV.z()},
-                    {"dTheta_X", deltaTheta.x()},
-                    {"dTheta_Y", deltaTheta.y()},
-                    {"dTheta_Z", deltaTheta.z()},
-                    {"Noisy_dV_X", imu_msg.linear_acceleration.x},
-                    {"Noisy_dV_Y", imu_msg.linear_acceleration.y},
-                    {"Noisy_dV_Z", imu_msg.linear_acceleration.z},
-                    {"Noisy_dTheta_X", imu_msg.angular_velocity.x},
-                    {"Noisy_dTheta_Y", imu_msg.angular_velocity.y},
-                    {"Noisy_dTheta_Z", imu_msg.angular_velocity.z}},
-                true, true);
+        csv.append(sc::CSV::Pairs{{"time", time_now},
+                                  {"dt", dt},
+                                  {"ECEF_POSX", ecefPos.x()},
+                                  {"ECEF_POSY", ecefPos.y()},
+                                  {"ECEF_POSZ", ecefPos.z()},
+                                  {"ECEF_VELX", ecefVel.x()},
+                                  {"ECEF_VELY", ecefVel.y()},
+                                  {"ECEF_VELZ", ecefVel.z()},
+                                  {"bodyToEcef_X", qbodyToECEF.x()},
+                                  {"bodyToEcef_Y", qbodyToECEF.y()},
+                                  {"bodyToEcef_Z", qbodyToECEF.z()},
+                                  {"bodyToEcef_W", qbodyToECEF.w()},
+                                  {"dV_X", deltaV.x()},
+                                  {"dV_Y", deltaV.y()},
+                                  {"dV_Z", deltaV.z()},
+                                  {"dTheta_X", deltaTheta.x()},
+                                  {"dTheta_Y", deltaTheta.y()},
+                                  {"dTheta_Z", deltaTheta.z()},
+                                  {"Noisy_dV_X", imu_msg.linear_acceleration.x},
+                                  {"Noisy_dV_Y", imu_msg.linear_acceleration.y},
+                                  {"Noisy_dV_Z", imu_msg.linear_acceleration.z},
+                                  {"Noisy_dTheta_X", imu_msg.angular_velocity.x},
+                                  {"Noisy_dTheta_Y", imu_msg.angular_velocity.y},
+                                  {"Noisy_dTheta_Z", imu_msg.angular_velocity.z}},
+                   true, true);
     }
     return true;
 }
@@ -453,5 +480,5 @@ bool ROSIMUSensor::step() {
 void ROSIMUSensor::close(double t) {
     csv.close_output();
 }
-} // namespace sensor
-} // namespace scrimmage
+}  // namespace sensor
+}  // namespace scrimmage
diff --git a/src/plugins/sensor/RayTrace/RayTrace.cpp b/src/plugins/sensor/RayTrace/RayTrace.cpp
index 598d09c418..c5b1d65ac7 100644
--- a/src/plugins/sensor/RayTrace/RayTrace.cpp
+++ b/src/plugins/sensor/RayTrace/RayTrace.cpp
@@ -32,16 +32,15 @@
 
 #include <scrimmage/common/Random.h>
 #include <scrimmage/entity/Entity.h>
-#include <scrimmage/math/State.h>
 #include <scrimmage/math/Angles.h>
 #include <scrimmage/math/Quaternion.h>
+#include <scrimmage/math/State.h>
 #include <scrimmage/parse/ParseUtils.h>
 #include <scrimmage/plugin_manager/RegisterPlugin.h>
+#include <scrimmage/plugins/sensor/RayTrace/RayTrace.h>
+#include <scrimmage/proto/State.pb.h>
 #include <scrimmage/pubsub/Message.h>
 #include <scrimmage/pubsub/Publisher.h>
-#include <scrimmage/proto/State.pb.h>
-
-#include <scrimmage/plugins/sensor/RayTrace/RayTrace.h>
 
 namespace sc = scrimmage;
 
@@ -52,16 +51,17 @@ namespace sensor {
 
 std::vector<RayTrace::PCRay> RayTrace::PointCloud::get_rays() {
     std::vector<RayTrace::PCRay> return_rays;
-    for (auto &ray : rays) {
+    for (auto& ray : rays) {
         // Make a copy so can be used cleanly
         return_rays.push_back(RayTrace::PCRay(ray.azimuth_rad, ray.elevation_rad));
     }
     return return_rays;
 }
 
-RayTrace::RayTrace() : max_range_(0), min_range_(0), max_sample_rate_(0) {}
+RayTrace::RayTrace() : max_range_(0), min_range_(0), max_sample_rate_(0) {
+}
 
-void RayTrace::init(std::map<std::string, std::string> &params) {
+void RayTrace::init(std::map<std::string, std::string>& params) {
     // Use the same generator as the parent so that the simulation is
     // completely deterministic with respect to the simulation seed.
     gener_ = parent_->random()->gener();
@@ -118,7 +118,7 @@ bool RayTrace::step() {
 
 std::vector<RayTrace::PCRay> RayTrace::rays() {
     std::vector<RayTrace::PCRay> return_rays;
-    for (auto &ray : rays_) {
+    for (auto& ray : rays_) {
         // Make a copy so can be used cleanly
         return_rays.push_back(RayTrace::PCRay(ray.azimuth_rad, ray.elevation_rad));
     }
diff --git a/src/plugins/sensor/RigidBody6DOFStateSensor/RigidBody6DOFStateSensor.cpp b/src/plugins/sensor/RigidBody6DOFStateSensor/RigidBody6DOFStateSensor.cpp
index a94ed37f66..1d6718515a 100644
--- a/src/plugins/sensor/RigidBody6DOFStateSensor/RigidBody6DOFStateSensor.cpp
+++ b/src/plugins/sensor/RigidBody6DOFStateSensor/RigidBody6DOFStateSensor.cpp
@@ -35,10 +35,10 @@
 #include <scrimmage/math/Quaternion.h>
 #include <scrimmage/math/State.h>
 #include <scrimmage/parse/ParseUtils.h>
-#include <scrimmage/plugins/sensor/RigidBody6DOFStateSensor/RigidBody6DOFStateSensor.h>
-#include <scrimmage/plugins/motion/RigidBody6DOF/RigidBody6DOFState.h>
-#include <scrimmage/plugins/motion/RigidBody6DOF/RigidBody6DOFBase.h>
 #include <scrimmage/plugin_manager/RegisterPlugin.h>
+#include <scrimmage/plugins/motion/RigidBody6DOF/RigidBody6DOFBase.h>
+#include <scrimmage/plugins/motion/RigidBody6DOF/RigidBody6DOFState.h>
+#include <scrimmage/plugins/sensor/RigidBody6DOFStateSensor/RigidBody6DOFStateSensor.h>
 #include <scrimmage/proto/State.pb.h>
 #include <scrimmage/pubsub/Message.h>
 #include <scrimmage/pubsub/Publisher.h>
@@ -48,8 +48,7 @@
 using std::cout;
 using std::endl;
 
-REGISTER_PLUGIN(scrimmage::Sensor,
-                scrimmage::sensor::RigidBody6DOFStateSensor,
+REGISTER_PLUGIN(scrimmage::Sensor, scrimmage::sensor::RigidBody6DOFStateSensor,
                 RigidBody6DOFStateSensor_plugin)
 
 namespace scrimmage {
@@ -58,7 +57,7 @@ namespace sensor {
 RigidBody6DOFStateSensor::RigidBody6DOFStateSensor() {
 }
 
-void RigidBody6DOFStateSensor::init(std::map<std::string, std::string> &params) {
+void RigidBody6DOFStateSensor::init(std::map<std::string, std::string>& params) {
     // Use the same generator as the parent so that the simulation is
     // completely deterministic with respect to the simulation seed.
     gener_ = parent_->random()->gener();
@@ -83,7 +82,8 @@ void RigidBody6DOFStateSensor::init(std::map<std::string, std::string> &params)
 
 bool RigidBody6DOFStateSensor::step() {
     if (motion_ == nullptr) {
-        motion_ = std::dynamic_pointer_cast<scrimmage::motion::RigidBody6DOFBase>(parent_->motion());
+        motion_ =
+            std::dynamic_pointer_cast<scrimmage::motion::RigidBody6DOFBase>(parent_->motion());
         if (motion_ == nullptr) {
             cout << "WARNING: Failed to get motion model. Currently only "
                  << "scrimmage::motion::RigidBody6DOFBase and subclasses "
@@ -100,8 +100,10 @@ bool RigidBody6DOFStateSensor::step() {
     msg->data.ang_vel() = parent_->state_truth()->ang_vel();
     msg->data.quat() = parent_->state_truth()->quat();
 
-    msg->data.linear_vel_body() = parent_->state_truth()->quat().rotate_reverse(parent_->state_truth()->vel());
-    msg->data.ang_vel_body()    = parent_->state_truth()->quat().rotate_reverse(parent_->state_truth()->ang_vel());
+    msg->data.linear_vel_body() =
+        parent_->state_truth()->quat().rotate_reverse(parent_->state_truth()->vel());
+    msg->data.ang_vel_body() =
+        parent_->state_truth()->quat().rotate_reverse(parent_->state_truth()->ang_vel());
 
     msg->data.linear_accel_body() = motion_->linear_accel_body();
     msg->data.ang_accel_body() = motion_->ang_accel_body();
@@ -112,5 +114,5 @@ bool RigidBody6DOFStateSensor::step() {
     pub_->publish(msg);
     return true;
 }
-} // namespace sensor
-} // namespace scrimmage
+}  // namespace sensor
+}  // namespace scrimmage
diff --git a/src/plugins/sensor/ScrimmageOpenAISensor/ScrimmageOpenAISensor.cpp b/src/plugins/sensor/ScrimmageOpenAISensor/ScrimmageOpenAISensor.cpp
index 2535587ed2..33a5b87375 100644
--- a/src/plugins/sensor/ScrimmageOpenAISensor/ScrimmageOpenAISensor.cpp
+++ b/src/plugins/sensor/ScrimmageOpenAISensor/ScrimmageOpenAISensor.cpp
@@ -30,17 +30,17 @@
  *
  */
 
-#include <scrimmage/plugins/sensor/ScrimmageOpenAISensor/ScrimmageOpenAISensor.h>
 #include <scrimmage/plugin_manager/RegisterPlugin.h>
+#include <scrimmage/plugins/sensor/ScrimmageOpenAISensor/ScrimmageOpenAISensor.h>
 
-REGISTER_PLUGIN(scrimmage::Sensor,
-                scrimmage::sensor::ScrimmageOpenAISensor,
+REGISTER_PLUGIN(scrimmage::Sensor, scrimmage::sensor::ScrimmageOpenAISensor,
                 ScrimmageOpenAISensor_plugin)
 
 namespace scrimmage {
 namespace sensor {
 
-ScrimmageOpenAISensor::ScrimmageOpenAISensor() : Sensor() {}
+ScrimmageOpenAISensor::ScrimmageOpenAISensor() : Sensor() {
+}
 
-} // namespace sensor
-} // namespace scrimmage
+}  // namespace sensor
+}  // namespace scrimmage
diff --git a/src/plugins/sensor/SimpleINS/SimpleINS.cpp b/src/plugins/sensor/SimpleINS/SimpleINS.cpp
index dafcb8f9dd..4ca7122e27 100644
--- a/src/plugins/sensor/SimpleINS/SimpleINS.cpp
+++ b/src/plugins/sensor/SimpleINS/SimpleINS.cpp
@@ -35,13 +35,13 @@
 #include <scrimmage/entity/Entity.h>
 #include <scrimmage/math/Quaternion.h>
 #include <scrimmage/math/State.h>
+#include <scrimmage/math/StateWithCovariance.h>
+#include <scrimmage/msgs/GPS.pb.h>
 #include <scrimmage/parse/ParseUtils.h>
-#include <scrimmage/pubsub/Message.h>
-#include <scrimmage/pubsub/Publisher.h>
 #include <scrimmage/plugin_manager/RegisterPlugin.h>
 #include <scrimmage/plugins/sensor/SimpleINS/SimpleINS.h>
-#include <scrimmage/math/StateWithCovariance.h>
-#include <scrimmage/msgs/GPS.pb.h>
+#include <scrimmage/pubsub/Message.h>
+#include <scrimmage/pubsub/Publisher.h>
 
 #include <vector>
 
@@ -53,15 +53,16 @@ REGISTER_PLUGIN(scrimmage::Sensor, scrimmage::sensor::SimpleINS, SimpleINS_plugi
 namespace scrimmage {
 namespace sensor {
 
-void SimpleINS::init(std::map<std::string, std::string> &params) {
+void SimpleINS::init(std::map<std::string, std::string>& params) {
     // Noise params information
-    auto add_noise = [&](std::string prefix, auto &noise_vec) {
+    auto add_noise = [&](std::string prefix, auto& noise_vec) {
         for (int i = 0; i < 3; i++) {
             std::string tag_name = prefix + "_" + std::to_string(i);
             std::vector<double> vec;
             double mean, stdev;
-            std::tie(mean, stdev) = get_vec(tag_name, params, " ", vec, 2) ?
-                std::make_pair(vec[0], vec[1]) : std::make_pair(0.0, 1.0);
+            std::tie(mean, stdev) = get_vec(tag_name, params, " ", vec, 2)
+                                        ? std::make_pair(vec[0], vec[1])
+                                        : std::make_pair(0.0, 1.0);
             noise_vec.push_back(parent_->random()->make_rng_normal(mean, stdev));
         }
     };
@@ -70,8 +71,6 @@ void SimpleINS::init(std::map<std::string, std::string> &params) {
     add_noise("vel_noise", vel_noise_);
     add_noise("orient_noise", orient_noise_);
 
-
-
     pub_ = advertise("LocalNetwork", "StateWithCovariance");
     pub_cep_ = advertise("LocalNetwork", "INS_CEP");
     pub_seastate_ = advertise("LocalNetwork", "INS_SS");
@@ -80,19 +79,15 @@ void SimpleINS::init(std::map<std::string, std::string> &params) {
     sea_state_gps_ = sc::get<double>("sea_state_gps", params, 99);
 
     // GPS information
-    auto gps_cb = [&](auto &msg) {
-        gps_fix_ = msg->data.fixed();
-    };
+    auto gps_cb = [&](auto& msg) { gps_fix_ = msg->data.fixed(); };
     subscribe<sm::GPSStatus>("GlobalNetwork", "GPSStatus", gps_cb);
 
-
     surface_timer_ = sc::get<double>("surface_timer", params, 10);
 
     parent_->state() = std::make_shared<State>();
     *(parent_->state()) = *(parent_->state_truth());
 }
 
-
 bool SimpleINS::step() {
     auto gener = parent_->random()->gener();
     auto msg_SS = std::make_shared<sc::Message<double>>();
@@ -109,26 +104,24 @@ bool SimpleINS::step() {
         init_m_ = false;
     }
 
-
-      // Acceleration
-        accel_ = ns.vel() - vel_Nminus1;
-        vel_Nminus1 = ns.vel();
+    // Acceleration
+    accel_ = ns.vel() - vel_Nminus1;
+    vel_Nminus1 = ns.vel();
     for (int i = 0; i < 3; i++) {
         accel_(i) = abs(accel_(i) / .01);
         if (accel_(i) > 1) {
             accel_(i) = 1;
-            }
+        }
     }
 
-
     // Use gen in order to create a growing covariance
     double pos_noise_0 = (*pos_noise_[0])(*gener) * (accel_(0));
     double pos_noise_1 = (*pos_noise_[1])(*gener) * (accel_(1));
     const int arrayNum[4] = {15, 30, 45, 60};
 
     if (!gps_fix_) {
-        m_(0, 0) += (pos_noise_0 < 0 ? -1*pos_noise_0 : pos_noise_0);
-        m_(1, 1) += (pos_noise_1 < 0 ? -1*pos_noise_1 : pos_noise_1);
+        m_(0, 0) += (pos_noise_0 < 0 ? -1 * pos_noise_0 : pos_noise_0);
+        m_(1, 1) += (pos_noise_1 < 0 ? -1 * pos_noise_1 : pos_noise_1);
 
         prev_time_ = time_->t();
         int randIdx = rand() % 4;
@@ -138,31 +131,28 @@ bool SimpleINS::step() {
         }
     } else {
         // GPS fix on - wait on surface timer and snap back to an identity matrix
-        m_(0, 0) += (pos_noise_0 < 0 ? -1*pos_noise_0 : pos_noise_0);
-        m_(1, 1) += (pos_noise_1 < 0 ? -1*pos_noise_1 : pos_noise_1);
+        m_(0, 0) += (pos_noise_0 < 0 ? -1 * pos_noise_0 : pos_noise_0);
+        m_(1, 1) += (pos_noise_1 < 0 ? -1 * pos_noise_1 : pos_noise_1);
         // GPS fix on - wait on surface timer and snap back to an identity matrix
         if (time_->t() - prev_time_ > surface_timer_) {
-           m_ = Eigen::MatrixXd::Identity(ns.covariance().rows(), ns.covariance().cols());
-           // Reset the position error accumulator
-           pos_error_accum_ = Eigen::Vector3d::Zero();
+            m_ = Eigen::MatrixXd::Identity(ns.covariance().rows(), ns.covariance().cols());
+            // Reset the position error accumulator
+            pos_error_accum_ = Eigen::Vector3d::Zero();
         }
     }
 
-
-
     // Create noisy position / velocity
     for (int i = 0; i < 3; i++) {
-         // std::cout << "Accelration for " << i << ", is: " << (accel_(i)) << std::endl;
-         pos_error_accum_(i) += (*pos_noise_[i])(*gener) * (accel_(i));
-         // msg->data.pos()(i) = ns.pos()(i) + (*pos_noise_[i])(*gener);
+        // std::cout << "Accelration for " << i << ", is: " << (accel_(i)) << std::endl;
+        pos_error_accum_(i) += (*pos_noise_[i])(*gener) * (accel_(i));
+        // msg->data.pos()(i) = ns.pos()(i) + (*pos_noise_[i])(*gener);
         msg->data.vel()(i) = ns.vel()(i) + (*vel_noise_[i])(*gener);
     }
-      msg->data.pos() = ns.pos() + pos_error_accum_;
+    msg->data.pos() = ns.pos() + pos_error_accum_;
 
-    msg->data.quat() = ns.quat()
-    * Quaternion(Eigen::Vector3d::UnitX(), (*orient_noise_[0])(*gener))
-    * Quaternion(Eigen::Vector3d::UnitY(), (*orient_noise_[1])(*gener))
-    * Quaternion(Eigen::Vector3d::UnitZ(), (*orient_noise_[2])(*gener));
+    msg->data.quat() = ns.quat() * Quaternion(Eigen::Vector3d::UnitX(), (*orient_noise_[0])(*gener))
+                       * Quaternion(Eigen::Vector3d::UnitY(), (*orient_noise_[1])(*gener))
+                       * Quaternion(Eigen::Vector3d::UnitZ(), (*orient_noise_[2])(*gener));
 
     msg->data.set_covariance(m_);
 
@@ -184,8 +174,8 @@ bool SimpleINS::step() {
         CEP = 0.59 * sig_L * (1 + sig_w);
     }
     auto msg_cep = std::make_shared<sc::Message<double>>();
-        msg_cep->data = CEP;
-        pub_cep_->publish(msg_cep);
+    msg_cep->data = CEP;
+    pub_cep_->publish(msg_cep);
     // Publish StateWCovariance msg
     pub_->publish(msg);
 
@@ -195,5 +185,5 @@ bool SimpleINS::step() {
     return true;
 }
 
-} // namespace sensor
-} // namespace scrimmage
+}  // namespace sensor
+}  // namespace scrimmage
diff --git a/src/proto_conversions/ProtoConversions.cpp b/src/proto_conversions/ProtoConversions.cpp
index 54a6a85a54..cc19d811a4 100644
--- a/src/proto_conversions/ProtoConversions.cpp
+++ b/src/proto_conversions/ProtoConversions.cpp
@@ -30,69 +30,69 @@
  *
  */
 
-#include <scrimmage/common/ID.h>
 #include <scrimmage/common/ColorMaps.h>
+#include <scrimmage/common/ID.h>
 #include <scrimmage/entity/Contact.h>
+#include <scrimmage/entity/EntityPlugin.h>
 #include <scrimmage/log/Frame.h>
 #include <scrimmage/math/State.h>
-#include <scrimmage/entity/EntityPlugin.h>
+#include <scrimmage/proto/Frame.pb.h>
 #include <scrimmage/proto/ProtoConversions.h>
 #include <scrimmage/proto/Shape.pb.h>
-#include <scrimmage/proto/Frame.pb.h>
 
 #include <cmath>
 
 namespace scrimmage {
 
-void set(scrimmage_proto::Vector3d *dst, Eigen::Vector3d src) {
+void set(scrimmage_proto::Vector3d* dst, Eigen::Vector3d src) {
     dst->set_x(src(0));
     dst->set_y(src(1));
     dst->set_z(src(2));
 }
 
-void set(scrimmage_proto::Vector3d *dst, double x, double y, double z) {
+void set(scrimmage_proto::Vector3d* dst, double x, double y, double z) {
     dst->set_x(x);
     dst->set_y(y);
     dst->set_z(z);
 }
 
-void set(Eigen::Vector3d &dst, const scrimmage_proto::Color &src) {
+void set(Eigen::Vector3d& dst, const scrimmage_proto::Color& src) {
     dst << src.r(), src.g(), src.b();
 }
 
-void set(std::vector<int> &dst, const scrimmage_proto::Color &src) {
+void set(std::vector<int>& dst, const scrimmage_proto::Color& src) {
     dst = {src.r(), src.g(), src.b()};
 }
 
-void set(scrimmage_proto::Color *dst, const std::vector<int> &src) {
+void set(scrimmage_proto::Color* dst, const std::vector<int>& src) {
     dst->set_r(src[0]);
     dst->set_g(src[1]);
     dst->set_b(src[2]);
 }
 
-void set(Eigen::Vector3d &dst, scrimmage_proto::Vector3d *src) {
+void set(Eigen::Vector3d& dst, scrimmage_proto::Vector3d* src) {
     dst(0) = src->x();
     dst(1) = src->y();
     dst(2) = src->z();
 }
 
-void set(scrimmage_proto::Color &dst, const scrimmage_proto::Color &src) {
+void set(scrimmage_proto::Color& dst, const scrimmage_proto::Color& src) {
     dst.set_r(src.r());
     dst.set_g(src.g());
     dst.set_b(src.b());
 }
 
-void set(scrimmage_proto::Color *dst, const Eigen::Vector3d &color) {
-     set(dst, color(0), color(1), color(2));
+void set(scrimmage_proto::Color* dst, const Eigen::Vector3d& color) {
+    set(dst, color(0), color(1), color(2));
 }
 
-void set(scrimmage_proto::Color &dst, scrimmage_proto::Color &src) {
+void set(scrimmage_proto::Color& dst, scrimmage_proto::Color& src) {
     dst.set_r(src.r());
     dst.set_g(src.g());
     dst.set_b(src.b());
 }
 
-void set(scrimmage_proto::Color &dst, int r, int g, int b) {
+void set(scrimmage_proto::Color& dst, int r, int g, int b) {
     scrimmage_proto::Color src;
     src.set_r(r);
     src.set_g(g);
@@ -100,23 +100,23 @@ void set(scrimmage_proto::Color &dst, int r, int g, int b) {
     set(dst, src);
 }
 
-void set(scrimmage_proto::Color *dst, scrimmage_proto::Color *src) {
+void set(scrimmage_proto::Color* dst, scrimmage_proto::Color* src) {
     *dst = *src;
 }
 
-void set(scrimmage_proto::Color *dst, scrimmage_proto::Color src) {
+void set(scrimmage_proto::Color* dst, scrimmage_proto::Color src) {
     dst->set_r(src.r());
     dst->set_g(src.g());
     dst->set_b(src.b());
 }
 
-void set(scrimmage_proto::Color *dst, const scrimmage::Color_t &src) {
+void set(scrimmage_proto::Color* dst, const scrimmage::Color_t& src) {
     dst->set_r(src.r);
     dst->set_g(src.g);
     dst->set_b(src.b);
 }
 
-void set(scrimmage_proto::Color *color, int r, int g, int b) {
+void set(scrimmage_proto::Color* color, int r, int g, int b) {
     scrimmage::Color_t c;
     c.r = r;
     c.g = g;
@@ -125,59 +125,59 @@ void set(scrimmage_proto::Color *color, int r, int g, int b) {
     set(color, c);
 }
 
-void set(scrimmage_proto::Color *color, int grayscale) {
+void set(scrimmage_proto::Color* color, int grayscale) {
     Color_t c = GetColor_matlab(grayscale, 0, 255);
     set(color, c);
 }
 
-void set(scrimmage_proto::Quaternion *dst, const Quaternion &src) {
+void set(scrimmage_proto::Quaternion* dst, const Quaternion& src) {
     dst->set_x(src.x());
     dst->set_y(src.y());
     dst->set_z(src.z());
     dst->set_w(src.w());
 }
 
-void set(scrimmage_proto::Quaternion *dst, const double &w, const double &x,
-         const double &y, const double &z) {
+void set(scrimmage_proto::Quaternion* dst, const double& w, const double& x, const double& y,
+         const double& z) {
     dst->set_x(x);
     dst->set_y(y);
     dst->set_z(z);
     dst->set_w(w);
 }
 
-void set(scrimmage_proto::State *dst, const scrimmage::StatePtr &state) {
+void set(scrimmage_proto::State* dst, const scrimmage::StatePtr& state) {
     set(dst->mutable_position(), state->pos());
     set(dst->mutable_linear_velocity(), state->vel());
     set(dst->mutable_angular_velocity(), state->ang_vel());
     set(dst->mutable_orientation(), state->quat());
 }
 
-void set(scrimmage_proto::State *dst, const scrimmage::State &state) {
+void set(scrimmage_proto::State* dst, const scrimmage::State& state) {
     set(dst->mutable_position(), state.pos_const());
     set(dst->mutable_linear_velocity(), state.vel_const());
     set(dst->mutable_angular_velocity(), state.ang_vel_const());
     set(dst->mutable_orientation(), state.quat_const());
 }
 
-void set(scrimmage::Quaternion &dst, const scrimmage_proto::Quaternion &quat) {
+void set(scrimmage::Quaternion& dst, const scrimmage_proto::Quaternion& quat) {
     dst.set(quat.w(), quat.x(), quat.y(), quat.z());
 }
 
-void set(scrimmage::State &dst, const scrimmage_proto::State &state) {
+void set(scrimmage::State& dst, const scrimmage_proto::State& state) {
     set(dst.pos(), state.position());
     set(dst.vel(), state.linear_velocity());
     set(dst.ang_vel(), state.angular_velocity());
     set(dst.quat(), state.orientation());
 }
 
-void set(scrimmage_proto::State *dst, scrimmage::State &state) {
+void set(scrimmage_proto::State* dst, scrimmage::State& state) {
     set(dst->mutable_position(), state.pos());
     set(dst->mutable_linear_velocity(), state.vel());
     set(dst->mutable_angular_velocity(), state.ang_vel());
     set(dst->mutable_orientation(), state.quat());
 }
 
-Eigen::Vector3d eigen(const scrimmage_proto::Vector3d &src) {
+Eigen::Vector3d eigen(const scrimmage_proto::Vector3d& src) {
     Eigen::Vector3d dst;
     dst(0) = src.x();
     dst(1) = src.y();
@@ -185,44 +185,42 @@ Eigen::Vector3d eigen(const scrimmage_proto::Vector3d &src) {
     return dst;
 }
 
-void add_point_color(std::shared_ptr<scrimmage_proto::PointCloud> s,
-                     const scrimmage::Color_t &c) {
-    scrimmage_proto::Color *color = s->add_color();
+void add_point_color(std::shared_ptr<scrimmage_proto::PointCloud> s, const scrimmage::Color_t& c) {
+    scrimmage_proto::Color* color = s->add_color();
     set(color, c);
 }
 
-void add_point_color(std::shared_ptr<scrimmage_proto::PointCloud> s,
-                     const int &r, const int &g, const int &b) {
-    scrimmage_proto::Color *color = s->add_color();
+void add_point_color(std::shared_ptr<scrimmage_proto::PointCloud> s, const int& r, const int& g,
+                     const int& b) {
+    scrimmage_proto::Color* color = s->add_color();
     set(color, r, g, b);
 }
 
-void add_point_color(std::shared_ptr<scrimmage_proto::PointCloud> s,
-                     const int &grayscale) {
-    scrimmage_proto::Color *color = s->add_color();
+void add_point_color(std::shared_ptr<scrimmage_proto::PointCloud> s, const int& grayscale) {
+    scrimmage_proto::Color* color = s->add_color();
     set(color, grayscale);
 }
 
-void set(scrimmage::ID &id, const scrimmage_proto::ID &proto_id) {
+void set(scrimmage::ID& id, const scrimmage_proto::ID& proto_id) {
     id.set_id(proto_id.id());
     id.set_sub_swarm_id(proto_id.sub_swarm_id());
     id.set_team_id(proto_id.team_id());
 }
 
-Eigen::Vector3d proto_2_vector3d(const scrimmage_proto::Vector3d &proto_vector3d) {
+Eigen::Vector3d proto_2_vector3d(const scrimmage_proto::Vector3d& proto_vector3d) {
     return Eigen::Vector3d(proto_vector3d.x(), proto_vector3d.y(), proto_vector3d.z());
 }
 
-Quaternion proto_2_quat(const scrimmage_proto::Quaternion &proto_quat) {
+Quaternion proto_2_quat(const scrimmage_proto::Quaternion& proto_quat) {
     const Quaternion quat(proto_quat.w(), proto_quat.x(), proto_quat.y(), proto_quat.z());
     return quat;
 }
 
-void set(Eigen::Vector3d &dst, const scrimmage_proto::Vector3d &proto_vector3d) {
+void set(Eigen::Vector3d& dst, const scrimmage_proto::Vector3d& proto_vector3d) {
     dst << proto_vector3d.x(), proto_vector3d.y(), proto_vector3d.z();
 }
 
-StatePtr proto_2_state(const scrimmage_proto::State &proto_state) {
+StatePtr proto_2_state(const scrimmage_proto::State& proto_state) {
     State state;
     set(state.pos(), proto_state.position());
     set(state.vel(), proto_state.linear_velocity());
@@ -231,7 +229,7 @@ StatePtr proto_2_state(const scrimmage_proto::State &proto_state) {
     return std::make_shared<State>(state);
 }
 
-void path_to_lines(std::vector<Eigen::Vector3d> &path,
+void path_to_lines(std::vector<Eigen::Vector3d>& path,
                    std::shared_ptr<scrimmage_proto::Shape> sample_line,
                    std::shared_ptr<EntityPlugin> p) {
     for (size_t i = 0; i < path.size() - 1; i++) {
@@ -243,9 +241,7 @@ void path_to_lines(std::vector<Eigen::Vector3d> &path,
     }
 }
 
-std::list<scrimmage_proto::Line> points_to_lines(
-    const std::list<Eigen::Vector3d> &points) {
-
+std::list<scrimmage_proto::Line> points_to_lines(const std::list<Eigen::Vector3d>& points) {
     // Stop one item before the end of the points list
     auto stop = std::prev(points.end());
 
@@ -259,51 +255,50 @@ std::list<scrimmage_proto::Line> points_to_lines(
     return lines;
 }
 
-
-Frame proto_2_frame(const scrimmage_proto::Frame &proto_frame) {
+Frame proto_2_frame(const scrimmage_proto::Frame& proto_frame) {
     Frame frame;
 
     frame.contacts_ = std::make_shared<ContactMap>();
     frame.time_ = proto_frame.time();
-    ContactMap &contacts = *(frame.contacts_);
+    ContactMap& contacts = *(frame.contacts_);
 
     for (int i = 0; i < proto_frame.contact_size(); i++) {
-        contacts[proto_frame.contact(i).id().id()] =
-                proto_2_contact(proto_frame.contact(i));
+        contacts[proto_frame.contact(i).id().id()] = proto_2_contact(proto_frame.contact(i));
     }
     return frame;
 }
 
-std::shared_ptr<scrimmage_proto::Frame> create_frame(double time, std::shared_ptr<ContactMap> &contacts) {
+std::shared_ptr<scrimmage_proto::Frame> create_frame(double time,
+                                                     std::shared_ptr<ContactMap>& contacts) {
     std::shared_ptr<scrimmage_proto::Frame> frame(new scrimmage_proto::Frame());
     frame->set_time(time);
 
-    for (auto &kv : *contacts) {
-        StatePtr &state = kv.second.state();
+    for (auto& kv : *contacts) {
+        StatePtr& state = kv.second.state();
         Contact::Type type = kv.second.type();
-        const ID &id = kv.second.id();
+        const ID& id = kv.second.id();
 
-        scrimmage_proto::Contact *contact = frame->add_contact();
-        scrimmage_proto::ID *sp_id = contact->mutable_id();
+        scrimmage_proto::Contact* contact = frame->add_contact();
+        scrimmage_proto::ID* sp_id = contact->mutable_id();
 
         set(contact->mutable_state(), state);
 
         switch (type) {
-        case Contact::Type::AIRCRAFT:
-            contact->set_type(scrimmage_proto::AIRCRAFT);
-            break;
-        case Contact::Type::QUADROTOR:
-            contact->set_type(scrimmage_proto::QUADROTOR);
-            break;
-        case Contact::Type::SPHERE:
-            contact->set_type(scrimmage_proto::SPHERE);
-            break;
-        case Contact::Type::MESH:
-            contact->set_type(scrimmage_proto::MESH);
-            break;
-        default:
-            contact->set_type(scrimmage_proto::UNKNOWN);
-            break;
+            case Contact::Type::AIRCRAFT:
+                contact->set_type(scrimmage_proto::AIRCRAFT);
+                break;
+            case Contact::Type::QUADROTOR:
+                contact->set_type(scrimmage_proto::QUADROTOR);
+                break;
+            case Contact::Type::SPHERE:
+                contact->set_type(scrimmage_proto::SPHERE);
+                break;
+            case Contact::Type::MESH:
+                contact->set_type(scrimmage_proto::MESH);
+                break;
+            default:
+                contact->set_type(scrimmage_proto::UNKNOWN);
+                break;
         }
 
         contact->set_active(kv.second.active());
@@ -315,7 +310,7 @@ std::shared_ptr<scrimmage_proto::Frame> create_frame(double time, std::shared_pt
     return frame;
 }
 
-Contact proto_2_contact(const scrimmage_proto::Contact &proto_contact) {
+Contact proto_2_contact(const scrimmage_proto::Contact& proto_contact) {
     Contact contact;
     set(contact.id(), proto_contact.id());
 
@@ -338,4 +333,4 @@ Contact proto_2_contact(const scrimmage_proto::Contact &proto_contact) {
     return contact;
 }
 
-} // namespace scrimmage
+}  // namespace scrimmage
diff --git a/src/pubsub/MessageBase.cpp b/src/pubsub/MessageBase.cpp
index a0712593a8..85f18db930 100644
--- a/src/pubsub/MessageBase.cpp
+++ b/src/pubsub/MessageBase.cpp
@@ -31,6 +31,7 @@
  */
 
 #include <scrimmage/pubsub/MessageBase.h>
+
 #include <cmath>
 
 #if ENABLE_PYTHON_BINDINGS == 1
@@ -52,5 +53,4 @@ namespace scrimmage {
 // }
 #endif
 
-
-} // namespace scrimmage
+}  // namespace scrimmage
diff --git a/src/pubsub/Network.cpp b/src/pubsub/Network.cpp
index 706bba29c7..85a18d11bb 100644
--- a/src/pubsub/Network.cpp
+++ b/src/pubsub/Network.cpp
@@ -34,50 +34,48 @@
 #include <scrimmage/common/Time.h>
 #include <scrimmage/entity/Entity.h>
 #include <scrimmage/msgs/Simple.pb.h>
-#include <scrimmage/parse/ParseUtils.h>
 #include <scrimmage/parse/MissionParse.h>
+#include <scrimmage/parse/ParseUtils.h>
 #include <scrimmage/pubsub/Network.h>
 #include <scrimmage/pubsub/NetworkDevice.h>
 #include <scrimmage/pubsub/Publisher.h>
 #include <scrimmage/pubsub/SubscriberBase.h>
 
-#include <memory>
 #include <iostream>
+#include <memory>
 
 namespace scrimmage {
 
-Network::Network() : EntityPlugin(), rtree_(std::make_shared<RTree>()) {}
+Network::Network() : EntityPlugin(), rtree_(std::make_shared<RTree>()) {
+}
 
-bool Network::init(std::map<std::string, std::string> &mission_params,
-                   std::map<std::string, std::string> &plugin_params) {
+bool Network::init(std::map<std::string, std::string>& mission_params,
+                   std::map<std::string, std::string>& plugin_params) {
     return network_init(mission_params, plugin_params);
 }
 
-bool Network::network_init(std::map<std::string, std::string> &mission_params,
-                           std::map<std::string, std::string> &plugin_params) {
-
-    auto setup_counts = [] (std::string str,
-                            std::map<std::string, std::string> &plugin_params,
-                            std::map<std::string, unsigned int> &counts,
-                            bool &monitor_all) {
+bool Network::network_init(std::map<std::string, std::string>& mission_params,
+                           std::map<std::string, std::string>& plugin_params) {
+    auto setup_counts = [](std::string str, std::map<std::string, std::string>& plugin_params,
+                           std::map<std::string, unsigned int>& counts, bool& monitor_all) {
         std::string topics_str = get<std::string>(str, plugin_params, "");
 
         auto topics = str2container<std::vector<std::string>>(topics_str, ", ");
 
-        if (std::end(topics) != std::find(std::begin(topics),
-                                          std::end(topics), "*")) {
+        if (std::end(topics) != std::find(std::begin(topics), std::end(topics), "*")) {
             counts["*"] = 0;
             monitor_all = true;
         }
 
-        for (std::string const &topic : topics) {
+        for (std::string const& topic : topics) {
             counts[topic] = 0;
         }
     };
 
     // Get communication delay for the network (default is 0)
     comm_delay_ = scrimmage::get<double>("comm_delay", plugin_params, comm_delay_);
-    is_stochastic_delay_ = scrimmage::get<bool>("is_stochastic_delay", plugin_params, is_stochastic_delay_);
+    is_stochastic_delay_ =
+        scrimmage::get<bool>("is_stochastic_delay", plugin_params, is_stochastic_delay_);
 
     setup_counts("monitor_publisher_topics", plugin_params, pub_counts_, monitor_all_pubs_);
     setup_counts("monitor_subscriber_topics", plugin_params, sub_counts_, monitor_all_subs_);
@@ -90,10 +88,10 @@ bool Network::network_init(std::map<std::string, std::string> &mission_params,
         csv_.open_output(mp_->log_dir() + "/" + filename);
 
         std::string headers = "t,";
-        for (auto &kv : pub_counts_) {
+        for (auto& kv : pub_counts_) {
             headers += kv.first + "_Pub_Count,";
         }
-        for (auto &kv : sub_counts_) {
+        for (auto& kv : sub_counts_) {
             headers += kv.first + "_Sub_Count,";
         }
 
@@ -102,15 +100,15 @@ bool Network::network_init(std::map<std::string, std::string> &mission_params,
     return true;
 }
 
-bool Network::step(std::map<std::string, std::list<NetworkDevicePtr>> &pubs,
-                   std::map<std::string, std::list<NetworkDevicePtr>> &subs) {
+bool Network::step(std::map<std::string, std::list<NetworkDevicePtr>>& pubs,
+                   std::map<std::string, std::list<NetworkDevicePtr>>& subs) {
     reachable_map_.clear();
 
     // Reset msg pub / sub counts
-    for (auto &kv : pub_counts_) {
+    for (auto& kv : pub_counts_) {
         kv.second = 0;
     }
-    for (auto &kv : sub_counts_) {
+    for (auto& kv : sub_counts_) {
         kv.second = 0;
     }
 
@@ -118,12 +116,11 @@ bool Network::step(std::map<std::string, std::list<NetworkDevicePtr>> &pubs,
     auto it_all_pub = pub_counts_.find("*");
     auto it_all_sub = sub_counts_.find("*");
 
-
     // number of messages delivered (may be >1 if delivered with delay)
     int n_delivered = 0;
 
     // For all publisher topic names
-    for (auto &pub_kv : pubs) {
+    for (auto& pub_kv : pubs) {
         std::string topic = pub_kv.first;
 
         // Get reference to this specific topic in the pub_counts_ map for
@@ -137,7 +134,7 @@ bool Network::step(std::map<std::string, std::list<NetworkDevicePtr>> &pubs,
         bool valid_sub_topic = (it_sub_topic != sub_counts_.end());
 
         // For all publisher devices with topic name
-        for (NetworkDevicePtr &pub : pub_kv.second) {
+        for (NetworkDevicePtr& pub : pub_kv.second) {
             pub->enforce_queue_size();
 
             auto msgs = pub->pop_msgs<MessageBase>();
@@ -156,8 +153,7 @@ bool Network::step(std::map<std::string, std::list<NetworkDevicePtr>> &pubs,
             }
 
             // For all subscribers on this topic
-            for (NetworkDevicePtr &sub : subs[topic]) {
-
+            for (NetworkDevicePtr& sub : subs[topic]) {
                 if (sub->undelivered_msg_list_size() > 0) {
                     // deliver undelivered messages if delay time has passed
                     n_delivered = sub->deliver_undelivered_msg(time_->t(), is_stochastic_delay_);
@@ -172,14 +168,11 @@ bool Network::step(std::map<std::string, std::list<NetworkDevicePtr>> &pubs,
                 }
 
                 if (is_reachable(pub->plugin(), sub->plugin())) {
-                    for (auto &msg : msgs) {
-                        if (is_successful_transmission(pub->plugin(),
-                                                       sub->plugin())) {
-
+                    for (auto& msg : msgs) {
+                        if (is_successful_transmission(pub->plugin(), sub->plugin())) {
                             double msg_delay = get_transmission_delay();
 
                             if (msg_delay < 0) {
-
                                 msg->time = time_->t();
                                 sub->add_msg(msg);
 
@@ -194,7 +187,7 @@ bool Network::step(std::map<std::string, std::list<NetworkDevicePtr>> &pubs,
                                 }
                             } else {
                                 // put msg in subs undelivered msg queue
-                                msg->time = time_->t()+msg_delay;
+                                msg->time = time_->t() + msg_delay;
                                 sub->add_undelivered_msg(msg, is_stochastic_delay_);
                             }
                         }
@@ -205,9 +198,9 @@ bool Network::step(std::map<std::string, std::list<NetworkDevicePtr>> &pubs,
     }
 
     // Enforce queue sizes, if necessary
-    for (auto &sub_kv : subs) {
+    for (auto& sub_kv : subs) {
         // For all publisher devices with topic name
-        for (NetworkDevicePtr &sub : sub_kv.second) {
+        for (NetworkDevicePtr& sub : sub_kv.second) {
             sub->enforce_queue_size();
         }
     }
@@ -216,12 +209,12 @@ bool Network::step(std::map<std::string, std::list<NetworkDevicePtr>> &pubs,
         CSV::Pairs pairs;
         pairs.push_back(std::make_pair("t", time_->t()));
 
-        for (auto &kv : pub_counts_) {
+        for (auto& kv : pub_counts_) {
             std::string header = kv.first + "_Pub_Count";
             pairs.push_back(std::make_pair(header, kv.second));
         }
 
-        for (auto &kv : sub_counts_) {
+        for (auto& kv : sub_counts_) {
             std::string header = kv.first + "_Sub_Count";
             pairs.push_back(std::make_pair(header, kv.second));
         }
@@ -231,14 +224,13 @@ bool Network::step(std::map<std::string, std::list<NetworkDevicePtr>> &pubs,
     return true;
 }
 
-
-bool Network::is_reachable(const scrimmage::EntityPluginPtr &pub_plugin,
-                           const scrimmage::EntityPluginPtr &sub_plugin) {
+bool Network::is_reachable(const scrimmage::EntityPluginPtr& pub_plugin,
+                           const scrimmage::EntityPluginPtr& sub_plugin) {
     return false;
 }
 
-bool Network::is_successful_transmission(const scrimmage::EntityPluginPtr &pub_plugin,
-                                         const scrimmage::EntityPluginPtr &sub_plugin) {
+bool Network::is_successful_transmission(const scrimmage::EntityPluginPtr& pub_plugin,
+                                         const scrimmage::EntityPluginPtr& sub_plugin) {
     return false;
 }
 
@@ -250,7 +242,7 @@ std::string Network::type() {
     return std::string("Network");
 }
 
-void Network::set_rtree(const RTreePtr &rtree) {
+void Network::set_rtree(const RTreePtr& rtree) {
     rtree_ = rtree;
 }
 
@@ -265,4 +257,4 @@ void Network::close(double t) {
     mp_ = nullptr;
 }
 
-} // namespace scrimmage
+}  // namespace scrimmage
diff --git a/src/pubsub/NetworkDevice.cpp b/src/pubsub/NetworkDevice.cpp
index 81e1aa2ea6..10c126562f 100644
--- a/src/pubsub/NetworkDevice.cpp
+++ b/src/pubsub/NetworkDevice.cpp
@@ -30,38 +30,45 @@
  *
  */
 
-#include <scrimmage/pubsub/NetworkDevice.h>
-#include <scrimmage/pubsub/MessageBase.h>
 #include <scrimmage/entity/EntityPlugin.h>
+#include <scrimmage/pubsub/MessageBase.h>
+#include <scrimmage/pubsub/NetworkDevice.h>
 
 #include <iostream>
 
 namespace scrimmage {
 
-NetworkDevice::NetworkDevice() : plugin_(std::make_shared<EntityPlugin>()) {}
+NetworkDevice::NetworkDevice() : plugin_(std::make_shared<EntityPlugin>()) {
+}
 
-NetworkDevice::NetworkDevice(const std::string &topic, const unsigned int& max_queue_size,
-                             const bool& enable_queue_size, EntityPluginPtr plugin) :
-    topic_(topic), max_queue_size_(max_queue_size),
-    enable_queue_size_(enable_queue_size), plugin_(plugin) {
+NetworkDevice::NetworkDevice(const std::string& topic, const unsigned int& max_queue_size,
+                             const bool& enable_queue_size, EntityPluginPtr plugin)
+    : topic_(topic),
+      max_queue_size_(max_queue_size),
+      enable_queue_size_(enable_queue_size),
+      plugin_(plugin) {
 }
 
-NetworkDevice::NetworkDevice(NetworkDevice &rhs) :
-    topic_(rhs.topic_),
-    max_queue_size_(rhs.max_queue_size_),
-    plugin_(rhs.plugin_),
-    msg_list_(rhs.msg_list_) {}
+NetworkDevice::NetworkDevice(NetworkDevice& rhs)
+    : topic_(rhs.topic_),
+      max_queue_size_(rhs.max_queue_size_),
+      plugin_(rhs.plugin_),
+      msg_list_(rhs.msg_list_) {
+}
 
-NetworkDevice::NetworkDevice(NetworkDevice &&rhs) :
-    topic_(rhs.topic_), max_queue_size_(rhs.max_queue_size_),
-    msg_list_(rhs.msg_list_) {
+NetworkDevice::NetworkDevice(NetworkDevice&& rhs)
+    : topic_(rhs.topic_), max_queue_size_(rhs.max_queue_size_), msg_list_(rhs.msg_list_) {
 }
 
-std::string NetworkDevice::get_topic() const {return topic_;}
+std::string NetworkDevice::get_topic() const {
+    return topic_;
+}
 
-void NetworkDevice::set_topic(const std::string &topic) {topic_ = topic;}
+void NetworkDevice::set_topic(const std::string& topic) {
+    topic_ = topic;
+}
 
-void NetworkDevice::set_msg_list(const std::list<MessageBasePtr> &msg_list) {
+void NetworkDevice::set_msg_list(const std::list<MessageBasePtr>& msg_list) {
     mutex_.lock();
     msg_list_ = msg_list;
     mutex_.unlock();
@@ -113,15 +120,14 @@ void NetworkDevice::clear_msg_list() {
     mutex_.unlock();
 }
 
-void NetworkDevice::print_str(const std::string &msg) {
+void NetworkDevice::print_str(const std::string& msg) {
     std::cout << msg << std::endl;
 }
 
-EntityPluginPtr & NetworkDevice::plugin() {
+EntityPluginPtr& NetworkDevice::plugin() {
     return plugin_;
 }
 
-
 /* added for delay handling */
 void NetworkDevice::add_undelivered_msg(MessageBasePtr msg, const bool& is_stochastic_delay) {
     mutex_.lock();
@@ -143,7 +149,6 @@ void NetworkDevice::add_undelivered_msg(MessageBasePtr msg, const bool& is_stoch
     mutex_.unlock();
 }
 
-
 auto NetworkDevice::deliver_undelivered_msg(std::list<MessageBasePtr>::iterator it) {
     mutex_.lock();
     msg_list_.push_back(*it);
@@ -152,14 +157,13 @@ auto NetworkDevice::deliver_undelivered_msg(std::list<MessageBasePtr>::iterator
     return it;
 }
 
-
-int NetworkDevice::deliver_undelivered_msg(const double& time_now, const bool& is_stochastic_delay) {
+int NetworkDevice::deliver_undelivered_msg(const double& time_now,
+                                           const bool& is_stochastic_delay) {
     // number of messages delivered
     int n_delivered = 0;
 
     for (auto it = undelivered_msg_list_.begin(); it != undelivered_msg_list_.end(); /**/) {
-        if ( time_now >= (*it)->time ) {
-
+        if (time_now >= (*it)->time) {
             mutex_.lock();
             msg_list_.push_back(*it);
             it = undelivered_msg_list_.erase(it);
@@ -176,4 +180,4 @@ int NetworkDevice::deliver_undelivered_msg(const double& time_now, const bool& i
 }
 
 //
-} // namespace scrimmage
+}  // namespace scrimmage
diff --git a/src/pubsub/PubSub.cpp b/src/pubsub/PubSub.cpp
index a2c88f7d38..36aaf78cf0 100644
--- a/src/pubsub/PubSub.cpp
+++ b/src/pubsub/PubSub.cpp
@@ -34,9 +34,8 @@
 #include <scrimmage/pubsub/PubSub.h>
 #include <scrimmage/pubsub/Publisher.h>
 
-#include <iostream>
-
 #include <boost/optional.hpp>
+#include <iostream>
 
 using std::cout;
 using std::endl;
@@ -46,36 +45,33 @@ namespace scrimmage {
 PubSub::PubSub() {
 }
 
-void PubSub::add_network_name(const std::string &network_name) {
+void PubSub::add_network_name(const std::string& network_name) {
     pub_map_[network_name] = std::map<std::string, std::list<NetworkDevicePtr>>();
     sub_map_[network_name] = std::map<std::string, std::list<NetworkDevicePtr>>();
 }
 
-PublisherPtr PubSub::advertise(const std::string &network_name,
-                               const std::string &topic,
-                               const unsigned int& max_queue_size,
-                               const bool& enable_queue_size,
+PublisherPtr PubSub::advertise(const std::string& network_name, const std::string& topic,
+                               const unsigned int& max_queue_size, const bool& enable_queue_size,
                                EntityPluginPtr plugin) {
     if (pub_map_.count(network_name) == 0) {
-        cout << "WARNING: " << plugin->name()
-             << " - Publisher unable to connect to network ("
+        cout << "WARNING: " << plugin->name() << " - Publisher unable to connect to network ("
              << network_name << ") on topic (" << topic << ")" << endl;
     }
 
-    PublisherPtr pub = std::make_shared<Publisher>(topic, max_queue_size,
-                                                   enable_queue_size, plugin);
+    PublisherPtr pub =
+        std::make_shared<Publisher>(topic, max_queue_size, enable_queue_size, plugin);
     pub_map_[network_name][topic].push_back(pub);
     return pub;
 }
 
-boost::optional<std::list<NetworkDevicePtr>> PubSub::find_devices(
-    const std::string &network_name, const std::string &topic_name, TopicMap &devs) {
-
+boost::optional<std::list<NetworkDevicePtr>> PubSub::find_devices(const std::string& network_name,
+                                                                  const std::string& topic_name,
+                                                                  TopicMap& devs) {
     auto it_network = devs.find(network_name);
     if (it_network == devs.end()) {
-         cout << "Failed to find network while setting up device." << endl;
-         cout << "Network name: " << network_name << endl;
-         cout << "Topic name: " << topic_name << endl;
+        cout << "Failed to find network while setting up device." << endl;
+        cout << "Network name: " << network_name << endl;
+        cout << "Topic name: " << topic_name << endl;
     } else {
         auto it_topic_pub = it_network->second.find(topic_name);
         if (it_topic_pub == it_network->second.end()) {
@@ -89,17 +85,17 @@ boost::optional<std::list<NetworkDevicePtr>> PubSub::find_devices(
     return boost::none;
 }
 
-boost::optional<std::list<NetworkDevicePtr>> PubSub::find_pubs(
-    const std::string &network_name, const std::string &topic_name) {
+boost::optional<std::list<NetworkDevicePtr>> PubSub::find_pubs(const std::string& network_name,
+                                                               const std::string& topic_name) {
     return find_devices(network_name, topic_name, pub_map_);
 }
 
-boost::optional<std::list<NetworkDevicePtr>> PubSub::find_subs(
-    const std::string &network_name, const std::string &topic_name) {
+boost::optional<std::list<NetworkDevicePtr>> PubSub::find_subs(const std::string& network_name,
+                                                               const std::string& topic_name) {
     return find_devices(network_name, topic_name, sub_map_);
 }
 
-void PubSub::print_str(const std::string &s) {
+void PubSub::print_str(const std::string& s) {
     std::cout << s << std::endl;
 }
-} // namespace scrimmage
+}  // namespace scrimmage
diff --git a/src/pubsub/Publisher.cpp b/src/pubsub/Publisher.cpp
index 6b6b509eed..3bd7ae5b14 100644
--- a/src/pubsub/Publisher.cpp
+++ b/src/pubsub/Publisher.cpp
@@ -39,14 +39,14 @@ namespace scrimmage {
 Publisher::Publisher() {
 }
 
-Publisher::Publisher(const std::string &topic, const unsigned int& max_queue_size,
-                     const bool& enable_queue_size, EntityPluginPtr plugin) :
-    NetworkDevice(topic, max_queue_size, enable_queue_size, plugin) {}
+Publisher::Publisher(const std::string& topic, const unsigned int& max_queue_size,
+                     const bool& enable_queue_size, EntityPluginPtr plugin)
+    : NetworkDevice(topic, max_queue_size, enable_queue_size, plugin) {
+}
 
-void Publisher::set_debug_info(MessageBasePtr msg, const std::string &type) {
-      msg->debug_info = std::string("  publisher:  ")
-        + "type (" + type
-        + "), plugin (" + plugin_->name() + ")"
-        + "), id (" + std::to_string(plugin_->parent()->id().id()) + ")";
+void Publisher::set_debug_info(MessageBasePtr msg, const std::string& type) {
+    msg->debug_info = std::string("  publisher:  ") + "type (" + type + "), plugin ("
+                      + plugin_->name() + ")" + "), id ("
+                      + std::to_string(plugin_->parent()->id().id()) + ")";
 }
-} // namespace scrimmage
+}  // namespace scrimmage
diff --git a/src/pubsub/SubscriberBase.cpp b/src/pubsub/SubscriberBase.cpp
index 9cb89873b7..0d1bfa7f67 100644
--- a/src/pubsub/SubscriberBase.cpp
+++ b/src/pubsub/SubscriberBase.cpp
@@ -34,19 +34,17 @@
 #include <scrimmage/entity/EntityPlugin.h>
 #include <scrimmage/pubsub/SubscriberBase.h>
 
-#include <iostream>
 #include <iomanip>
+#include <iostream>
 
 namespace scrimmage {
 
-void SubscriberBase::print_err(const std::string &type, MessageBasePtr msg) const {
+void SubscriberBase::print_err(const std::string& type, MessageBasePtr msg) const {
     std::cout << "WARNING: could not cast on topic " << std::quoted(topic_) << std::endl;
-    std::cout << "  subscriber: type (" << type
-        << "), plugin (" << plugin_->name()
-        << "), id (" << plugin_->parent()->id().id() << ")"
-        << std::endl;
+    std::cout << "  subscriber: type (" << type << "), plugin (" << plugin_->name() << "), id ("
+              << plugin_->parent()->id().id() << ")" << std::endl;
     if (msg->debug_info != "") {
         std::cout << msg->debug_info << std::endl;
     }
 }
-} // namespace scrimmage
+}  // namespace scrimmage
diff --git a/src/sensor/Sensor.cpp b/src/sensor/Sensor.cpp
index 569b669ed7..540125b0c8 100644
--- a/src/sensor/Sensor.cpp
+++ b/src/sensor/Sensor.cpp
@@ -34,12 +34,16 @@
 
 namespace scrimmage {
 
-void Sensor::init(std::map<std::string, std::string> &params) {return;}
+void Sensor::init(std::map<std::string, std::string>& params) {
+    return;
+}
 
-std::string Sensor::type() {return std::string("Sensor");}
+std::string Sensor::type() {
+    return std::string("Sensor");
+}
 
 scrimmage::MessageBasePtr Sensor::sensor_msg(double t) {
     return nullptr;
 }
 
-} // namespace scrimmage
+}  // namespace scrimmage
diff --git a/src/simcontrol/SimControl.cpp b/src/simcontrol/SimControl.cpp
index 6e980af0aa..3a4bad5fb4 100644
--- a/src/simcontrol/SimControl.cpp
+++ b/src/simcontrol/SimControl.cpp
@@ -30,74 +30,68 @@
  *
  */
 
+#include <scrimmage/autonomy/Autonomy.h>
 #include <scrimmage/common/Algorithm.h>
+#include <scrimmage/common/GlobalService.h>
+#include <scrimmage/common/ParameterServer.h>
+#include <scrimmage/common/RTree.h>
 #include <scrimmage/common/Random.h>
-#include <scrimmage/parse/MissionParse.h>
-#include <scrimmage/log/Log.h>
-#include <scrimmage/log/Print.h>
-#include <scrimmage/metrics/Metrics.h>
-#include <scrimmage/plugin_manager/PluginManager.h>
-#include <scrimmage/network/Interface.h>
-#include <scrimmage/sensor/Sensor.h>
-#include <scrimmage/common/Utilities.h>
 #include <scrimmage/common/Time.h>
+#include <scrimmage/common/Utilities.h>
 #include <scrimmage/entity/Contact.h>
-#include <scrimmage/common/RTree.h>
-#include <scrimmage/common/ParameterServer.h>
-#include <scrimmage/common/GlobalService.h>
 #include <scrimmage/entity/Entity.h>
-#include <scrimmage/motion/MotionModel.h>
+#include <scrimmage/log/Log.h>
+#include <scrimmage/log/Print.h>
+#include <scrimmage/math/Angles.h>
+#include <scrimmage/math/State.h>
+#include <scrimmage/metrics/Metrics.h>
 #include <scrimmage/motion/Controller.h>
-#include <scrimmage/simcontrol/SimControl.h>
-#include <scrimmage/simcontrol/SimUtils.h>
-#include <scrimmage/simcontrol/EntityInteraction.h>
+#include <scrimmage/motion/MotionModel.h>
+#include <scrimmage/msgs/Event.pb.h>
+#include <scrimmage/network/Interface.h>
 #include <scrimmage/parse/ConfigParse.h>
+#include <scrimmage/parse/MissionParse.h>
 #include <scrimmage/parse/ParseUtils.h>
-#include <scrimmage/autonomy/Autonomy.h>
-
-#include <scrimmage/math/State.h>
-#include <scrimmage/math/Angles.h>
-
+#include <scrimmage/plugin_manager/PluginManager.h>
+#include <scrimmage/proto/Frame.pb.h>
 #include <scrimmage/proto/ProtoConversions.h>
 #include <scrimmage/proto/Visual.pb.h>
-#include <scrimmage/proto/Frame.pb.h>
-
-#include <scrimmage/pubsub/Publisher.h>
-#include <scrimmage/pubsub/Subscriber.h>
+#include <scrimmage/pubsub/Message.h>
 #include <scrimmage/pubsub/Network.h>
 #include <scrimmage/pubsub/PubSub.h>
-#include <scrimmage/pubsub/Message.h>
-
-#include <scrimmage/msgs/Event.pb.h>
+#include <scrimmage/pubsub/Publisher.h>
+#include <scrimmage/pubsub/Subscriber.h>
+#include <scrimmage/sensor/Sensor.h>
+#include <scrimmage/simcontrol/EntityInteraction.h>
+#include <scrimmage/simcontrol/SimControl.h>
+#include <scrimmage/simcontrol/SimUtils.h>
 
+#include <chrono>  // NOLINT
+#include <future>  // NOLINT
 #include <iostream>
-#include <string>
 #include <memory>
-#include <chrono> // NOLINT
-#include <future> // NOLINT
+#include <string>
 
 #if ENABLE_PYTHON_BINDINGS == 1
 #include <pybind11/pybind11.h>
 #ifdef __clang__
-_Pragma("clang diagnostic push")
-_Pragma("clang diagnostic ignored \"-Wmacro-redefined\"")
-_Pragma("clang diagnostic ignored \"-Wdeprecated-register\"")
+_Pragma("clang diagnostic push") _Pragma("clang diagnostic ignored \"-Wmacro-redefined\"")
+    _Pragma("clang diagnostic ignored \"-Wdeprecated-register\"")
 #endif
 #include <Python.h>
 #ifdef __clang__
-_Pragma("clang diagnostic pop")
+        _Pragma("clang diagnostic pop")
 #endif
 #endif
 
 #include <GeographicLib/LocalCartesian.hpp>
-
-#include <boost/thread.hpp>
 #include <boost/range/adaptor/map.hpp>
 #include <boost/range/adaptor/transformed.hpp>
 #include <boost/range/algorithm/for_each.hpp>
 #include <boost/range/numeric.hpp>
+#include <boost/thread.hpp>
 
-namespace sc = scrimmage;
+            namespace sc = scrimmage;
 namespace sp = scrimmage_proto;
 namespace sm = scrimmage_msgs;
 namespace br = boost::range;
@@ -109,26 +103,26 @@ using NormDistribution = std::normal_distribution<double>;
 
 namespace scrimmage {
 
-SimControl::SimControl() :
-    id_to_team_map_(std::make_shared<std::unordered_map<int, int>>()),
-    id_to_ent_map_(std::make_shared<std::unordered_map<int, EntityPtr>>()),
-    incoming_interface_(std::make_shared<Interface>()),
-    outgoing_interface_(std::make_shared<Interface>()),
-    mp_(std::make_shared<MissionParse>()),
-    time_(std::make_shared<Time>()),
-    param_server_(std::make_shared<ParameterServer>()),
-    global_services_(std::make_shared<GlobalService>()),
-    timer_(Timer()),
-    log_(std::make_shared<Log>()),
-    printer_(std::make_shared<Print>()),
-    random_(std::make_shared<Random>()),
-    plugin_manager_(std::make_shared<PluginManager>()),
-    networks_(std::make_shared<std::map<std::string, NetworkPtr>>()),
-    pubsub_(std::make_shared<PubSub>()),
-    file_search_(std::make_shared<FileSearch>()),
-    rtree_(std::make_shared<scrimmage::RTree>()),
-    sim_plugin_(std::make_shared<EntityPlugin>()),
-    limited_verbosity_(false) {
+SimControl::SimControl()
+    : id_to_team_map_(std::make_shared<std::unordered_map<int, int>>()),
+      id_to_ent_map_(std::make_shared<std::unordered_map<int, EntityPtr>>()),
+      incoming_interface_(std::make_shared<Interface>()),
+      outgoing_interface_(std::make_shared<Interface>()),
+      mp_(std::make_shared<MissionParse>()),
+      time_(std::make_shared<Time>()),
+      param_server_(std::make_shared<ParameterServer>()),
+      global_services_(std::make_shared<GlobalService>()),
+      timer_(Timer()),
+      log_(std::make_shared<Log>()),
+      printer_(std::make_shared<Print>()),
+      random_(std::make_shared<Random>()),
+      plugin_manager_(std::make_shared<PluginManager>()),
+      networks_(std::make_shared<std::map<std::string, NetworkPtr>>()),
+      pubsub_(std::make_shared<PubSub>()),
+      file_search_(std::make_shared<FileSearch>()),
+      rtree_(std::make_shared<scrimmage::RTree>()),
+      sim_plugin_(std::make_shared<EntityPlugin>()),
+      limited_verbosity_(false) {
     pause(false);
     prev_paused_ = false;
     single_step(false);
@@ -165,8 +159,7 @@ bool SimControl::setup_logging() {
     return true;
 }
 
-bool SimControl::init(const std::string& mission_file,
-                      const bool& init_python) {
+bool SimControl::init(const std::string& mission_file, const bool& init_python) {
 #if ENABLE_PYTHON_BINDINGS == 1
     if (init_python) {
         Py_Initialize();
@@ -220,7 +213,7 @@ bool SimControl::generate_entities(const double& t) {
     // rate properties. pair consists of entity description ID and
     // first_in_group.
     std::list<int> ents_to_gen;
-    for (auto &kv : mp_->entity_descriptions()) {
+    for (auto& kv : mp_->entity_descriptions()) {
         int ent_desc_id = kv.first;
 
         // Skip this entity description if all of its entities have already
@@ -232,9 +225,8 @@ bool SimControl::generate_entities(const double& t) {
         // Generate entities in this entity description block if time has been
         // reached
         int gen_count = 0;
-        for (double &gen_time : mp_->next_gen_times()[ent_desc_id]) {
-
-            GenerateInfo &gen_info = mp_->gen_info()[ent_desc_id];
+        for (double& gen_time : mp_->next_gen_times()[ent_desc_id]) {
+            GenerateInfo& gen_info = mp_->gen_info()[ent_desc_id];
             if (t < gen_time || gen_info.total_count <= 0) {
                 continue;
             }
@@ -262,22 +254,20 @@ bool SimControl::generate_entities(const double& t) {
         }
     }
     // Delete gen_info's that don't have ents remaining (count==0)
-    remove_if(mp_->gen_info(), [&](auto &kv) {return kv.second.total_count <= 0;});
+    remove_if(mp_->gen_info(), [&](auto& kv) { return kv.second.total_count <= 0; });
 
     // Create a new rtree from the existing entity map and provide additional
     // space in the rtree for the new entities that will be generated.
     create_rtree(ents_to_gen.size());
 
     // Call generate_entity on each entity description id.
-    auto gen_ent = [&] (const int &ent_desc_id) -> bool {
-        return generate_entity(ent_desc_id);
-    };
+    auto gen_ent = [&](const int& ent_desc_id) -> bool { return generate_entity(ent_desc_id); };
     bool status = std::all_of(ents_to_gen.begin(), ents_to_gen.end(), gen_ent);
 
     return status;
 }
 
-bool SimControl::generate_entity(const int &ent_desc_id) {
+bool SimControl::generate_entity(const int& ent_desc_id) {
     // Get the entity's params
     auto it_params = mp_->entity_descriptions().find(ent_desc_id);
     if (it_params == mp_->entity_descriptions().end()) {
@@ -290,9 +280,8 @@ bool SimControl::generate_entity(const int &ent_desc_id) {
     return generate_entity(ent_desc_id, it_params->second, plugin_attr_map);
 }
 
-bool SimControl::generate_entity(const int &ent_desc_id,
-                                 std::map<std::string, std::string> &params,
-                                 AttributeMap &plugin_attr_map) {
+bool SimControl::generate_entity(const int& ent_desc_id, std::map<std::string, std::string>& params,
+                                 AttributeMap& plugin_attr_map) {
 #if ENABLE_JSBSIM == 1
     params["JSBSIM_ROOT"] = jsbsim_root_;
 #endif
@@ -360,13 +349,10 @@ bool SimControl::generate_entity(const int &ent_desc_id,
 
     int id = find_available_id(params);
 
-    bool ent_status = ent->init(plugin_attr_map, params, id_to_team_map_,
-                                id_to_ent_map_,
-                                contacts_, mp_, proj_, id, ent_desc_id,
-                                plugin_manager_, file_search_, rtree_, pubsub_,
-                                printer_, time_, param_server_, global_services_,
-                                std::set<std::string>{},
-                                [](std::map<std::string, std::string>&){});
+    bool ent_status = ent->init(
+        plugin_attr_map, params, id_to_team_map_, id_to_ent_map_, contacts_, mp_, proj_, id,
+        ent_desc_id, plugin_manager_, file_search_, rtree_, pubsub_, printer_, time_, param_server_,
+        global_services_, std::set<std::string>{}, [](std::map<std::string, std::string>&) {});
     contacts_mutex_.unlock();
 
     if (!ent_status) {
@@ -392,9 +378,8 @@ bool SimControl::generate_entity(const int &ent_desc_id,
     ents_.push_back(ent);
     rtree_->add(ent->state()->pos(), ent->id());
     contacts_mutex_.lock();
-    (*contacts_)[ent->id().id()] =
-            Contact(ent->id(), ent->radius(), ent->state_truth(),
-                    ent->type(), ent->contact_visual(), ent->properties());
+    (*contacts_)[ent->id().id()] = Contact(ent->id(), ent->radius(), ent->state_truth(),
+                                           ent->type(), ent->contact_visual(), ent->properties());
     contacts_mutex_.unlock();
 
     auto msg = std::make_shared<Message<sm::EntityGenerated>>();
@@ -404,7 +389,9 @@ bool SimControl::generate_entity(const int &ent_desc_id,
     return true;
 }
 
-MissionParsePtr SimControl::mp() { return mp_; }
+MissionParsePtr SimControl::mp() {
+    return mp_;
+}
 
 bool SimControl::enable_gui() {
     return mp_->enable_gui();
@@ -420,15 +407,15 @@ void SimControl::join() {
 
 void SimControl::create_rtree(const unsigned int& additional_size) {
     rtree_->init(ents_.size() + additional_size);
-    for (EntityPtr &ent: ents_) {
+    for (EntityPtr& ent : ents_) {
         rtree_->add(ent->state()->pos(), ent->id());
     }
 }
 
 void SimControl::set_autonomy_contacts() {
     std::map<std::string, AutonomyPtr> autonomy_map;
-    for (EntityPtr &ent : ents_) {
-        for (AutonomyPtr &autonomy : ent->autonomies()) {
+    for (EntityPtr& ent : ents_) {
+        for (AutonomyPtr& autonomy : ent->autonomies()) {
             if (autonomy->need_reset()) {
                 std::string type = autonomy->type();
                 auto it = autonomy_map.find(type);
@@ -449,36 +436,32 @@ void SimControl::set_autonomy_contacts() {
 
 bool SimControl::run_networks() {
     bool all_true = true;
-    for (auto &kv : *networks_) {
-        bool result = kv.second->step(pubsub_->pubs()[kv.second->name()],
-                                      pubsub_->subs()[kv.second->name()]);
+    for (auto& kv : *networks_) {
+        bool result =
+            kv.second->step(pubsub_->pubs()[kv.second->name()], pubsub_->subs()[kv.second->name()]);
         if (!result && kv.second->print_err_on_exit) {
-            cout << "Network requested simulation termination: "
-                 << kv.second->name() << endl;
+            cout << "Network requested simulation termination: " << kv.second->name() << endl;
         }
         all_true &= result;
 
-        shapes_[0].insert(shapes_[0].end(), kv.second->shapes().begin(),
-                          kv.second->shapes().end());
+        shapes_[0].insert(shapes_[0].end(), kv.second->shapes().begin(), kv.second->shapes().end());
         kv.second->shapes().clear();
     }
     return all_true;
 }
 
 bool SimControl::run_interaction_detection() {
-
     auto run_interaction = [&](auto ent_inter) {
         bool result = ent_inter->step_entity_interaction(ents_, t_, dt_);
         if (!result && ent_inter->print_err_on_exit) {
-            cout << "Entity interaction requested simulation termination: "
-                 << ent_inter->name() << endl;
+            cout << "Entity interaction requested simulation termination: " << ent_inter->name()
+                 << endl;
         }
         return result;
     };
 
     auto handle_shapes = [&](auto ent_inter) {
-        shapes_[0].insert(shapes_[0].end(), ent_inter->shapes().begin(),
-                          ent_inter->shapes().end());
+        shapes_[0].insert(shapes_[0].end(), ent_inter->shapes().begin(), ent_inter->shapes().end());
         ent_inter->shapes().clear();
     };
 
@@ -487,7 +470,7 @@ bool SimControl::run_interaction_detection() {
     br::for_each(ent_inters_, handle_shapes);
 
     // Determine if entities need to be removed
-    for (auto &ent : ents_) {
+    for (auto& ent : ents_) {
         if (!ent->is_alive() && ent->posthumous(this->t())) {
             int id = ent->id().id();
 
@@ -511,15 +494,14 @@ bool SimControl::run_interaction_detection() {
 
 bool SimControl::run_metrics() {
     br::for_each(metrics_, run_callbacks);
-    auto run_metric = [&](auto &metric) {return metric->step_metrics(t_, dt_);};
+    auto run_metric = [&](auto& metric) { return metric->step_metrics(t_, dt_); };
     return std::all_of(metrics_.begin(), metrics_.end(), run_metric);
 }
 
 bool SimControl::run_logging() {
     contacts_mutex_.lock();
 
-    std::shared_ptr<scrimmage_proto::Frame> frame =
-        create_frame(t_ + dt_, contacts_);
+    std::shared_ptr<scrimmage_proto::Frame> frame = create_frame(t_ + dt_, contacts_);
 
     outgoing_interface_->send_frame(frame);
     log_->save_frame(frame);
@@ -543,8 +525,8 @@ void SimControl::run_remove_inactive() {
             // entity from the id_to_team_map.
             auto it_id_ent = id_to_ent_map_->find(id);
             if (it_id_ent == id_to_ent_map_->end()) {
-                cout << "WARNING: Failed to remove entity ID ("
-                     << id << ") from id_to_ent_map" << endl;
+                cout << "WARNING: Failed to remove entity ID (" << id << ") from id_to_ent_map"
+                     << endl;
             } else {
                 id_to_ent_map_->erase(it_id_ent);
             }
@@ -582,7 +564,6 @@ bool SimControl::run_single_step(const int& loop_number) {
     // Stay in loop if currently paused.
     bool exit_loop = false;
     do {
-
         // Were we told to exit, externally?
         exit_mutex_.lock();
         if (exit_) {
@@ -655,7 +636,7 @@ bool SimControl::run_single_step(const int& loop_number) {
 
     run_remove_inactive();
     run_send_shapes();
-    run_send_contact_visuals(); // send updated visuals
+    run_send_contact_visuals();  // send updated visuals
 
     if (display_progress_) {
         if (loop_number % 100 == 0) {
@@ -668,7 +649,7 @@ bool SimControl::run_single_step(const int& loop_number) {
     set_time(t + dt_);
     prev_paused_ = paused_;
 
-    return not (end_condition_reached() || exit_loop);
+    return not(end_condition_reached() || exit_loop);
 }
 
 void SimControl::run_threaded() {
@@ -696,18 +677,20 @@ bool SimControl::start() {
 
     display_progress_ = get("display_progress", mp_->params(), true);
 
-    proj_ = mp_->projection(); // get projection (origin) from mission
+    proj_ = mp_->projection();  // get projection (origin) from mission
 
     if (get("show_plugins", mp_->params(), false)) {
         plugin_manager_->print_plugins("scrimmage::Autonomy", "Autonomy Plugins", *file_search_);
         plugin_manager_->print_plugins("scrimmage::MotionModel", "Motion Plugins", *file_search_);
-        plugin_manager_->print_plugins("scrimmage::Controller", "Controller Plugins", *file_search_);
-        plugin_manager_->print_plugins("scrimmage::EntityInteraction", "Entity Interaction Plugins", *file_search_);
+        plugin_manager_->print_plugins("scrimmage::Controller", "Controller Plugins",
+                                       *file_search_);
+        plugin_manager_->print_plugins("scrimmage::EntityInteraction", "Entity Interaction Plugins",
+                                       *file_search_);
         plugin_manager_->print_plugins("scrimmage::Sensor", "Sensor Plugins", *file_search_);
         plugin_manager_->print_plugins("scrimmage::Metrics", "Metrics Plugins", *file_search_);
     }
 
-        // Setup random seed
+    // Setup random seed
     if (mp_->params().count("seed") > 0) {
         auto seed = std::stoul(mp_->params()["seed"]);
         random_->seed(seed);
@@ -726,9 +709,8 @@ bool SimControl::start() {
     }
     log_->write_ascii("Seed: " + std::to_string(random_->get_seed()));
 
-    auto get_count = [&](auto &kv) {return kv.second.total_count;};
-    int max_num_entities =
-        boost::accumulate(mp_->gen_info() | ba::transformed(get_count), 0);
+    auto get_count = [&](auto& kv) { return kv.second.total_count; };
+    int max_num_entities = boost::accumulate(mp_->gen_info() | ba::transformed(get_count), 0);
 
     // What is the end condition?
     if (mp_->params().count("end_condition") > 0) {
@@ -749,18 +731,14 @@ bool SimControl::start() {
         end_conditions_.insert(EndConditionFlags::TIME);
     }
 
-    if (mp_->params().count("stream_port") > 0 &&
-        mp_->params().count("stream_ip") > 0) {
-
+    if (mp_->params().count("stream_port") > 0 && mp_->params().count("stream_ip") > 0) {
         if (mp_->network_gui()) {
-            outgoing_interface_->init_network(Interface::client,
-                                              mp_->params()["stream_ip"],
+            outgoing_interface_->init_network(Interface::client, mp_->params()["stream_ip"],
                                               std::stoi(mp_->params()["stream_port"]));
 
-            network_thread_ = std::thread(&Interface::init_network, &(*incoming_interface_),
-                                          Interface::server,
-                                          "localhost",
-                                          std::stoi(mp_->params()["stream_port"])+1);
+            network_thread_ =
+                std::thread(&Interface::init_network, &(*incoming_interface_), Interface::server,
+                            "localhost", std::stoi(mp_->params()["stream_port"]) + 1);
             network_thread_.detach();
         } else {
             outgoing_interface_->set_mode(Interface::shared);
@@ -773,9 +751,8 @@ bool SimControl::start() {
     }
 
     // If the GlobalNetwork doesn't exist, add it.
-    auto it_global_network = std::find(mp_->network_names().begin(),
-                                       mp_->network_names().end(),
-                                       "GlobalNetwork");
+    auto it_global_network =
+        std::find(mp_->network_names().begin(), mp_->network_names().end(), "GlobalNetwork");
     if (it_global_network == mp_->network_names().end()) {
         mp_->network_names().push_back("GlobalNetwork");
     }
@@ -786,9 +763,9 @@ bool SimControl::start() {
     sim_plugin_->set_pubsub(pubsub_);
 
     // Create base shape objects
-    for (auto &kv : mp_->team_info()) {
+    for (auto& kv : mp_->team_info()) {
         int i = 0;
-        for (Eigen::Vector3d &base_pos : kv.second.bases) {
+        for (Eigen::Vector3d& base_pos : kv.second.bases) {
             auto base = std::make_shared<scrimmage_proto::Shape>();
             base->mutable_sphere()->set_radius(kv.second.radii[i]);
             sc::set(base->mutable_sphere()->mutable_center(), base_pos);
@@ -818,7 +795,8 @@ bool SimControl::start() {
     networks_ = std::make_shared<NetworkMap>();
     if (!create_networks(info, *networks_)) return false;
     if (!create_metrics(info, contacts_, metrics_)) return false;
-    if (!create_ent_inters(info, contacts_, shapes_[0], ent_inters_, global_services_)) return false;
+    if (!create_ent_inters(info, contacts_, shapes_[0], ent_inters_, global_services_))
+        return false;
 
     // Setup simcontrol's pubsub plugin
     pub_end_time_ = sim_plugin_->advertise("GlobalNetwork", "EndTime");
@@ -832,19 +810,18 @@ bool SimControl::start() {
     pub_custom_key_ = sim_plugin_->advertise("GlobalNetwork", "CustomKeyPress");
 
     // Set subscriber / callback that allows plugins to generate entities
-    auto gen_ent_cb = [&] (auto &msg) {
+    auto gen_ent_cb = [&](auto& msg) {
         auto it_ent_desc_id = mp_->entity_tag_to_id().find(msg->data.entity_tag());
         if (it_ent_desc_id == mp_->entity_tag_to_id().end()) {
-            cout << "ERROR: Failed to find entity_tag, "
-                 << msg->data.entity_tag() << ", in mission file." << endl;
+            cout << "ERROR: Failed to find entity_tag, " << msg->data.entity_tag()
+                 << ", in mission file." << endl;
             return;
         }
         // Get the vehicle's params block:
         auto it_params = mp_->entity_descriptions().find(it_ent_desc_id->second);
         if (it_params == mp_->entity_descriptions().end()) {
-            cout << "ERROR: Failed to find entity block id, "
-                 << it_ent_desc_id->second << ", for entity_tag: "
-                 << msg->data.entity_tag() << ", in mission file." << endl;
+            cout << "ERROR: Failed to find entity block id, " << it_ent_desc_id->second
+                 << ", for entity_tag: " << msg->data.entity_tag() << ", in mission file." << endl;
             return;
         }
 
@@ -872,33 +849,33 @@ bool SimControl::start() {
         }
 
         AttributeMap plugin_attr_map = mp_->entity_attributes()[it_ent_desc_id->second];
-        for (int i = 0; i < msg->data.plugin_param().size(); i++){
-            plugin_attr_map[msg->data.plugin_param(i).plugin_type()][msg->data.plugin_param(i).tag_name()] = msg->data.plugin_param(i).tag_value();
+        for (int i = 0; i < msg->data.plugin_param().size(); i++) {
+            plugin_attr_map[msg->data.plugin_param(i).plugin_type()]
+                           [msg->data.plugin_param(i).tag_name()] =
+                               msg->data.plugin_param(i).tag_value();
         }
 
         // Recreate the rtree with one additional size for this entity.
         this->create_rtree(1);
 
         if (not this->generate_entity(it_ent_desc_id->second, params, plugin_attr_map)) {
-            cout << "Failed to generate entity with tag: "
-                 << msg->data.entity_tag() << endl;
+            cout << "Failed to generate entity with tag: " << msg->data.entity_tag() << endl;
             return;
         }
     };
-    sim_plugin_->subscribe<sm::GenerateEntity>("GlobalNetwork",
-                                               "GenerateEntity", gen_ent_cb);
+    sim_plugin_->subscribe<sm::GenerateEntity>("GlobalNetwork", "GenerateEntity", gen_ent_cb);
 
     // Set subscriber / callback that allows plugins to take a screenshot of the GUI
     // if the enable_gui XML tag is set to true
-    auto takeSS = [&](auto &msg) {
-        if(enable_gui()){
+    auto takeSS = [&](auto& msg) {
+        if (enable_gui()) {
             request_screenshot();
         }
     };
     sim_plugin_->subscribe<bool>("GlobalNetwork", "take_screenshot", takeSS);
 
     contacts_mutex_.lock();
-    contacts_->reserve(max_num_entities+1);
+    contacts_->reserve(max_num_entities + 1);
     contacts_mutex_.unlock();
 
     if (get("show_plugins", mp_->params(), false)) {
@@ -908,7 +885,7 @@ bool SimControl::start() {
     if (get("multi_threaded", mp_->params(), false)) {
         auto it = mp_->attributes().find("multi_threaded");
         if (it != mp_->attributes().end()) {
-            auto &attr_map = it->second;
+            auto& attr_map = it->second;
             auto add = [&](auto attr_str, auto attr_type) {
                 if (str2bool(get(attr_str, attr_map, "true"))) {
                     entity_thread_types_.insert(attr_type);
@@ -1009,7 +986,8 @@ bool SimControl::run() {
         return false;
     }
     int loop_number = 0;
-    while (run_single_step(loop_number++)) {}
+    while (run_single_step(loop_number++)) {
+    }
     return finalize();
 }
 
@@ -1023,7 +1001,7 @@ bool SimControl::finalize() {
     if (!entity_thread_types_.empty()) {
         entity_pool_stop_ = true;
         entity_pool_condition_var_.notify_all();
-        for (std::thread &t : entity_worker_threads_) {
+        for (std::thread& t : entity_worker_threads_) {
             t.join();
         }
     }
@@ -1032,7 +1010,7 @@ bool SimControl::finalize() {
     set_time(t() - dt_);
 
     // Save EntityPresentAtEnd messages
-    for (EntityPtr &ent : ents_) {
+    for (EntityPtr& ent : ents_) {
         auto msg = std::make_shared<Message<sm::EntityPresentAtEnd>>();
         msg->data.set_entity_id(ent->id().id());
         pub_ent_pres_end_->publish(msg);
@@ -1073,7 +1051,7 @@ bool SimControl::shutdown(const bool& shutdown_python) {
     finalize();
 
     // Close all plugins
-    for (EntityPtr &ent : ents_) {
+    for (EntityPtr& ent : ents_) {
         ent->close(t());
     }
 
@@ -1081,7 +1059,7 @@ bool SimControl::shutdown(const bool& shutdown_python) {
         ent_inter->close_plugin(t());
     }
 
-    for (auto &kv : *networks_) {
+    for (auto& kv : *networks_) {
         kv.second->close_plugin(t());
     }
 
@@ -1151,8 +1129,8 @@ bool SimControl::wait_for_ready() {
     // Wait for all entities to be ready
     int not_ready_loop = 0;
     while (!not_ready_.empty()) {
-        for (std::list<EntityPtr>::iterator it = not_ready_.begin();
-             it != not_ready_.end(); /* No increment */) {
+        for (std::list<EntityPtr>::iterator it = not_ready_.begin(); it != not_ready_.end();
+             /* No increment */) {
             if ((*it)->ready()) {
                 // Remove references to entities that are ready
                 it = not_ready_.erase(it);
@@ -1185,7 +1163,6 @@ bool SimControl::wait_for_ready() {
 }
 
 bool SimControl::end_condition_reached() {
-
     if (end_conditions_.count(EndConditionFlags::TIME) && t() > mp_->tend() - dt_ / 2.0) {
         auto msg = std::make_shared<Message<sm::EndTime>>();
         pub_end_time_->publish(msg);
@@ -1195,33 +1172,42 @@ bool SimControl::end_condition_reached() {
     if (ents_.empty()) {
         auto msg = std::make_shared<Message<sm::NoTeamsPresent>>();
         pub_no_teams_->publish(msg);
-        if (end_conditions_.count(EndConditionFlags::ALL_DEAD) ||
-            end_conditions_.count(EndConditionFlags::ONE_TEAM)) {
-
+        if (end_conditions_.count(EndConditionFlags::ALL_DEAD)
+            || end_conditions_.count(EndConditionFlags::ONE_TEAM)) {
             std::cout << std::endl << "End of Simulation: No Entities Remaining" << std::endl;
             return true;
         }
     } else if (end_conditions_.count(EndConditionFlags::ONE_TEAM)) {
         const int team1_id = ents_.front()->id().team_id();
-        const bool all_same_team = std::all_of(ents_.rbegin(), ents_.rend(),
-            [&](EntityPtr &ent) {return team1_id == ent->id().team_id();});
+        const bool all_same_team = std::all_of(ents_.rbegin(), ents_.rend(), [&](EntityPtr& ent) {
+            return team1_id == ent->id().team_id();
+        });
         if (all_same_team) {
             auto msg = std::make_shared<Message<sm::OneTeamPresent>>();
             pub_one_team_->publish(msg);
-            std::cout << std::endl << "End of Simulation: One Team (" << team1_id << ")" << std::endl;
+            std::cout << std::endl
+                      << "End of Simulation: One Team (" << team1_id << ")" << std::endl;
             return true;
         }
     }
     return false;
 }
 
-Timer &SimControl::timer() {return timer_;}
+Timer& SimControl::timer() {
+    return timer_;
+}
 
-std::list<MetricsPtr> &SimControl::metrics() { return metrics_; }
+std::list<MetricsPtr>& SimControl::metrics() {
+    return metrics_;
+}
 
-PluginManagerPtr &SimControl::plugin_manager() {return plugin_manager_;}
+PluginManagerPtr& SimControl::plugin_manager() {
+    return plugin_manager_;
+}
 
-FileSearchPtr &SimControl::file_search() {return file_search_;}
+FileSearchPtr& SimControl::file_search() {
+    return file_search_;
+}
 
 bool SimControl::take_step() {
     take_step_mutex_.lock();
@@ -1230,17 +1216,19 @@ bool SimControl::take_step() {
     return value;
 }
 
-void SimControl::set_incoming_interface(InterfacePtr &incoming_interface)
-{ incoming_interface_ = incoming_interface; }
+void SimControl::set_incoming_interface(InterfacePtr& incoming_interface) {
+    incoming_interface_ = incoming_interface;
+}
 
-void SimControl::set_outgoing_interface(InterfacePtr &outgoing_interface)
-{ outgoing_interface_ = outgoing_interface; }
+void SimControl::set_outgoing_interface(InterfacePtr& outgoing_interface) {
+    outgoing_interface_ = outgoing_interface;
+}
 
 void SimControl::run_check_network_msgs() {
     // Do we have any simcontrol message updates from GUI?
     if (incoming_interface_->gui_msg_update()) {
         incoming_interface_->gui_msg_mutex.lock();
-        auto &control = incoming_interface_->gui_msg();
+        auto& control = incoming_interface_->gui_msg();
         auto it = control.begin();
         while (it != control.end()) {
             if (it->inc_warp()) {
@@ -1270,7 +1258,7 @@ void SimControl::run_check_network_msgs() {
 
     if (incoming_interface_->world_point_clicked_msg_update()) {
         incoming_interface_->world_point_clicked_msg_mutex.lock();
-        auto &msg_list = incoming_interface_->world_point_clicked_msg();
+        auto& msg_list = incoming_interface_->world_point_clicked_msg();
         auto it = msg_list.begin();
         while (it != msg_list.end()) {
             auto msg = std::make_shared<Message<sp::WorldPointClicked>>();
@@ -1282,9 +1270,9 @@ void SimControl::run_check_network_msgs() {
     }
 }
 
-bool SimControl::collision_exists(Eigen::Vector3d &p) {
+bool SimControl::collision_exists(Eigen::Vector3d& p) {
     return std::any_of(ent_inters_.begin(), ent_inters_.end(),
-        [&](auto ent_inter) {return ent_inter->collision_exists(ents_, p);});
+                       [&](auto ent_inter) { return ent_inter->collision_exists(ents_, p); });
 }
 
 void SimControl::force_exit() {
@@ -1308,18 +1296,18 @@ void SimControl::set_finished(bool finished) {
     finished_mutex_.unlock();
 }
 
-void SimControl::get_contacts(std::unordered_map<int, Contact> &contacts) {
+void SimControl::get_contacts(std::unordered_map<int, Contact>& contacts) {
     // The Viewer GUI also looks at the contacts
     contacts_mutex_.lock();
     contacts = *contacts_;
-    for (auto &kv : contacts) {
+    for (auto& kv : contacts) {
         sc::State state = *kv.second.state();
         *kv.second.state() = state;
     }
     contacts_mutex_.unlock();
 }
 
-void SimControl::get_contact_visuals(std::map<int, ContactVisualPtr> &contact_visuals) {
+void SimControl::get_contact_visuals(std::map<int, ContactVisualPtr>& contact_visuals) {
     contact_visuals = contact_visuals_;
 }
 
@@ -1358,7 +1346,9 @@ double SimControl::time_warp() {
     return warp;
 }
 
-double SimControl::actual_time_warp() { return -1; }
+double SimControl::actual_time_warp() {
+    return -1;
+}
 
 void SimControl::set_time(const double& t) {
     time_mutex_.lock();
@@ -1447,27 +1437,25 @@ void SimControl::worker() {
 
             bool success = false;
             if (task_type == Task::Type::AUTONOMY) {
-                auto &autonomies = ent->autonomies();
+                auto& autonomies = ent->autonomies();
                 br::for_each(autonomies, run_callbacks);
-                auto run = [&](auto &a) {
-                  return a->step_loop_timer(temp_dt) ?
-                    a->step_autonomy(temp_t, temp_dt) : true;};
+                auto run = [&](auto& a) {
+                    return a->step_loop_timer(temp_dt) ? a->step_autonomy(temp_t, temp_dt) : true;
+                };
                 success = std::all_of(autonomies.begin(), autonomies.end(), run);
             } else if (task_type == Task::Type::CONTROLLER) {
-                auto &controllers = ent->controllers();
+                auto& controllers = ent->controllers();
                 br::for_each(controllers, run_callbacks);
-                auto run = [&](auto &c) {
-                  return c->step_loop_timer(temp_dt) ?
-                    c->step(temp_t, temp_dt) : true;};
+                auto run = [&](auto& c) {
+                    return c->step_loop_timer(temp_dt) ? c->step(temp_t, temp_dt) : true;
+                };
                 success = std::all_of(controllers.begin(), controllers.end(), run);
             } else if (task_type == Task::Type::MOTION) {
                 success = ent->motion()->step(temp_t, temp_dt);
             } else if (task_type == Task::Type::SENSOR) {
                 auto sensors = ent->sensors() | ba::map_values;
                 br::for_each(sensors, run_callbacks);
-                auto run = [&](auto &s) {
-                  return s->step_loop_timer(temp_dt) ?
-                    s->step() : true;};
+                auto run = [&](auto& s) { return s->step_loop_timer(temp_dt) ? s->step() : true; };
                 success = std::all_of(sensors.begin(), sensors.end(), run);
             }
 
@@ -1480,9 +1468,9 @@ void SimControl::worker() {
 
 void print_err(EntityPluginPtr p) {
     if (p->print_err_on_exit) {
-        std::cout << "failed to update entity " << p->parent()->id().id()
-            << ", plugin type \"" << p->type() << "\""
-            << ", plugin name \"" << p->name() << "\"" << std::endl;
+        std::cout << "failed to update entity " << p->parent()->id().id() << ", plugin type \""
+                  << p->type() << "\""
+                  << ", plugin name \"" << p->name() << "\"" << std::endl;
     }
 }
 
@@ -1491,9 +1479,9 @@ bool SimControl::run_sensors() {
     if (entity_thread_types_.count(Task::Type::SENSOR)) {
         success &= add_tasks(Task::Type::SENSOR, t_, dt_);
     } else {
-        for (EntityPtr &ent : ents_) {
+        for (EntityPtr& ent : ents_) {
             br::for_each(ent->sensors() | ba::map_values, run_callbacks);
-            for (auto &sensor : ent->sensors() | ba::map_values) {
+            for (auto& sensor : ent->sensors() | ba::map_values) {
                 if (sensor->step_loop_timer(dt_)) {
                     if (!sensor->step()) {
                         print_err(sensor);
@@ -1504,9 +1492,9 @@ bool SimControl::run_sensors() {
         }
     }
 
-    for (EntityPtr &ent : ents_) {
-        auto &shapes = shapes_[ent->id().id()];
-        for (auto &sensor : ent->sensors() | ba::map_values) {
+    for (EntityPtr& ent : ents_) {
+        auto& shapes = shapes_[ent->id().id()];
+        for (auto& sensor : ent->sensors() | ba::map_values) {
             shapes.insert(shapes.end(), sensor->shapes().begin(), sensor->shapes().end());
             sensor->shapes().clear();
         }
@@ -1525,7 +1513,7 @@ bool SimControl::add_tasks(Task::Type type, double t, double dt) {
     futures.reserve(ents_.size());
 
     entity_pool_mutex_.lock();
-    for (EntityPtr &ent : ents_) {
+    for (EntityPtr& ent : ents_) {
         std::shared_ptr<Task> task = std::make_shared<Task>();
         task->ent = ent;
         task->type = type;
@@ -1540,7 +1528,7 @@ bool SimControl::add_tasks(Task::Type type, double t, double dt) {
     entity_pool_condition_var_.notify_all();
 
     // wait for results
-    auto get = [&](auto &future) {return future.get();};
+    auto get = [&](auto& future) { return future.get(); };
     return std::all_of(futures.begin(), futures.end(), get);
 }
 
@@ -1562,11 +1550,11 @@ bool SimControl::run_entities() {
     if (entity_thread_types_.count(Task::Type::AUTONOMY)) {
         success &= add_tasks(Task::Type::AUTONOMY, t_, dt_);
     } else {
-        for (EntityPtr &ent : ents_) {
+        for (EntityPtr& ent : ents_) {
             for (auto a : ent->autonomies()) {
-                success &= exec_step(a, [&](auto a){
-                  return a->step_loop_timer(dt_) ?
-                    a->step_autonomy(t_, dt_) : true;});
+                success &= exec_step(a, [&](auto a) {
+                    return a->step_loop_timer(dt_) ? a->step_autonomy(t_, dt_) : true;
+                });
             }
         }
     }
@@ -1575,11 +1563,10 @@ bool SimControl::run_entities() {
     double temp_t = t_;
     for (int i = 0; i < mp_->motion_multiplier(); i++) {
         // run controllers in a single thread since they are serially connected
-        for (EntityPtr &ent : ents_) {
+        for (EntityPtr& ent : ents_) {
             for (auto c : ent->controllers()) {
-                success &= exec_step(c, [&](auto c){
-                  return c->step_loop_timer(dt_) ?
-                    c->step(t_, dt_) : true;});
+                success &= exec_step(
+                    c, [&](auto c) { return c->step_loop_timer(dt_) ? c->step(t_, dt_) : true; });
             }
         }
 
@@ -1588,36 +1575,35 @@ bool SimControl::run_entities() {
             if (entity_thread_types_.count(type)) {
                 success &= add_tasks(type, temp_t, motion_dt);
             } else {
-                for (EntityPtr &ent : ents_) {
-                    auto step = [&](auto p){return p->step(temp_t, motion_dt);};
+                for (EntityPtr& ent : ents_) {
+                    auto step = [&](auto p) { return p->step(temp_t, motion_dt); };
                     success &= exec_step(getter(ent), step);
                 }
             }
         };
-        step_all(Task::Type::MOTION, [&](auto ent){return ent->motion();});
+        step_all(Task::Type::MOTION, [&](auto ent) { return ent->motion(); });
 
         temp_t += motion_dt;
     }
 
     // Check if any entity has NaN in its state
-    for (EntityPtr &ent : ents_) {
+    for (EntityPtr& ent : ents_) {
         if (ent->state()->pos().hasNaN()) {
-            cout << "WARNING: Entity with motion model, "
-                 << ent->motion()->name() << ", contains a NaN value." << endl
+            cout << "WARNING: Entity with motion model, " << ent->motion()->name()
+                 << ", contains a NaN value." << endl
                  << "Check your time step values and for NaN values coming "
-                 << "from Autonomy and Controller plugins."
-                 << endl;
+                 << "from Autonomy and Controller plugins." << endl;
             cout << "Removing entity ID: " << ent->id().id() << endl;
             ent->collision();
         }
     }
 
-    for (EntityPtr &ent : ents_) {
+    for (EntityPtr& ent : ents_) {
         ent->setup_desired_state();
     }
 
-    for (EntityPtr &ent : ents_) {
-        for (AutonomyPtr &autonomy : ent->autonomies()) {
+    for (EntityPtr& ent : ents_) {
+        for (AutonomyPtr& autonomy : ent->autonomies()) {
             if (autonomy->need_reset()) {
                 autonomy->set_state(ent->motion()->state());
             }
@@ -1626,9 +1612,9 @@ bool SimControl::run_entities() {
 
     contacts_mutex_.unlock();
 
-    for (EntityPtr &ent : ents_) {
-        auto &shapes = shapes_[ent->id().id()];
-        auto add_shapes = [&](const auto &p) {
+    for (EntityPtr& ent : ents_) {
+        auto& shapes = shapes_[ent->id().id()];
+        auto add_shapes = [&](const auto& p) {
             shapes.insert(shapes.end(), p->shapes().begin(), p->shapes().end());
             p->shapes().clear();
         };
@@ -1643,9 +1629,9 @@ void SimControl::run_send_shapes() {
     // Convert map of shapes to sp::Shapes type
     scrimmage_proto::Shapes shapes;
     shapes.set_time(this->t());
-    for (auto &kv : shapes_) {
-        for (auto &shape : kv.second) {
-            scrimmage_proto::Shape *s = shapes.add_shape();
+    for (auto& kv : shapes_) {
+        for (auto& shape : kv.second) {
+            scrimmage_proto::Shape* s = shapes.add_shape();
             *s = *shape;
         }
     }
@@ -1655,7 +1641,7 @@ void SimControl::run_send_shapes() {
 }
 
 void SimControl::run_send_contact_visuals() {
-    for (EntityPtr &ent : ents_) {
+    for (EntityPtr& ent : ents_) {
         if (ent->visual_changed()) {
             outgoing_interface_->send_contact_visual(ent->contact_visual());
             log_->save_contact_visual(ent->contact_visual());
@@ -1677,17 +1663,16 @@ bool SimControl::output_runtime() {
 }
 
 bool SimControl::output_git_summary() {
-    std::map<std::string, std::unordered_set<std::string>> commits =
-            plugin_manager_->get_commits();
+    std::map<std::string, std::unordered_set<std::string>> commits = plugin_manager_->get_commits();
     std::string scrimmage_version = get_version();
 
     if (scrimmage_version != "") {
         commits[scrimmage_version].insert("scrimmage");
     }
 
-    for (auto &kv : commits) {
+    for (auto& kv : commits) {
         std::string output = kv.first + ":";
-        for (const std::string &plugin_name : kv.second) {
+        for (const std::string& plugin_name : kv.second) {
             output += plugin_name + ",";
         }
         output.pop_back();
@@ -1715,14 +1700,14 @@ bool SimControl::output_summary() {
 
         // Add all elements from individual metrics plugin to overall
         // metrics data structure
-        for (auto const &team_str_double : metrics->team_metrics()) {
+        for (auto const& team_str_double : metrics->team_metrics()) {
             team_metrics[team_str_double.first].insert(team_str_double.second.begin(),
                                                        team_str_double.second.end());
             metrics_empty = false;
         }
 
         // Calculate aggregated team scores:
-        for (auto const &team_score : metrics->team_scores()) {
+        for (auto const& team_score : metrics->team_scores()) {
             if (team_scores.count(team_score.first) == 0) {
                 team_scores[team_score.first] = 0;
             }
@@ -1730,8 +1715,7 @@ bool SimControl::output_summary() {
         }
 
         // Create list of all csv headers
-        headers.insert(headers.end(), metrics->headers().begin(),
-                       metrics->headers().end());
+        headers.insert(headers.end(), metrics->headers().begin(), metrics->headers().end());
     }
 
     // Create headers string
@@ -1742,8 +1726,7 @@ bool SimControl::output_summary() {
     csv_str += "\n";
 
     // Loop over each team and generate csv output
-    for (auto const &team_str_double : team_metrics) {
-
+    for (auto const& team_str_double : team_metrics) {
         // Each line starts with team_id,score
         csv_str += std::to_string(team_str_double.first);
         csv_str += "," + std::to_string(team_scores[team_str_double.first]);
@@ -1767,8 +1750,7 @@ bool SimControl::output_summary() {
     std::string out_file = mp_->log_dir() + "/summary.csv";
     std::ofstream summary_file(out_file);
     if (!summary_file.is_open()) {
-        std::cout << "could not open " << out_file
-                  << " for writing metrics" << std::endl;
+        std::cout << "could not open " << out_file << " for writing metrics" << std::endl;
         return false;
     }
     summary_file << csv_str << std::flush;
@@ -1779,7 +1761,7 @@ bool SimControl::output_summary() {
         cout << sc::generate_chars("=", 80) << endl;
         cout << "Overall Scores" << endl;
         cout << sc::generate_chars("=", 80) << endl;
-        for (auto const &team_score : team_scores) {
+        for (auto const& team_score : team_scores) {
             cout << "Team ID: " << team_score.first << endl;
             cout << "Score: " << team_score.second << endl;
             cout << sc::generate_chars("-", 80) << endl;
@@ -1796,7 +1778,7 @@ InterfacePtr SimControl::outgoing_interface() {
     return outgoing_interface_;
 }
 
-std::list<EntityPtr> &SimControl::ents() {
+std::list<EntityPtr>& SimControl::ents() {
     return ents_;
 }
 
@@ -1804,14 +1786,11 @@ EntityPluginPtr SimControl::plugin() {
     return sim_plugin_;
 }
 
-std::shared_ptr<std::unordered_map<int, EntityPtr>>
-SimControl::id_to_entity_map() {
+std::shared_ptr<std::unordered_map<int, EntityPtr>> SimControl::id_to_entity_map() {
     return id_to_ent_map_;
 }
 
-int SimControl::find_available_id(
-    const std::map<std::string, std::string>& params) {
-
+int SimControl::find_available_id(const std::map<std::string, std::string>& params) {
     // Use the mission file specified ID, if it exists, otherwise, find the
     // next available ID from the back of the ids_used_ std::set.
     int id = 0;
@@ -1820,8 +1799,8 @@ int SimControl::find_available_id(
         try {
             id = std::stoi(it_id->second);
         } catch (...) {
-            cout << "Failed to convert the following <id> tag into an integer: "
-                 << it_id->second << endl;
+            cout << "Failed to convert the following <id> tag into an integer: " << it_id->second
+                 << endl;
             id = 0;
         }
     }
@@ -1836,4 +1815,4 @@ int SimControl::find_available_id(
     return id;
 }
 
-} // namespace scrimmage
+}  // namespace scrimmage
diff --git a/src/simcontrol/SimUtils.cpp b/src/simcontrol/SimUtils.cpp
index c0a3035949..6db3bc01a3 100644
--- a/src/simcontrol/SimUtils.cpp
+++ b/src/simcontrol/SimUtils.cpp
@@ -36,18 +36,17 @@
 #include <scrimmage/log/Log.h>
 #include <scrimmage/metrics/Metrics.h>
 #include <scrimmage/network/Interface.h>
-#include <scrimmage/parse/MissionParse.h>
 #include <scrimmage/parse/ConfigParse.h>
+#include <scrimmage/parse/MissionParse.h>
 #include <scrimmage/parse/ParseUtils.h>
 #include <scrimmage/plugin_manager/PluginManager.h>
 #include <scrimmage/pubsub/Network.h>
 #include <scrimmage/simcontrol/EntityInteraction.h>
 #include <scrimmage/simcontrol/SimControl.h>
 #include <scrimmage/simcontrol/SimUtils.h>
-
 #include <signal.h>
-#include <cstdlib>
 
+#include <cstdlib>
 #include <iostream>
 
 using std::cout;
@@ -55,34 +54,29 @@ using std::endl;
 
 namespace scrimmage {
 
-bool create_ent_inters(const SimUtilsInfo &info,
-                       ContactMapPtr contacts,
-                       std::list<scrimmage_proto::ShapePtr> &shapes,
-                       std::list<EntityInteractionPtr> &ent_inters,
+bool create_ent_inters(const SimUtilsInfo& info, ContactMapPtr contacts,
+                       std::list<scrimmage_proto::ShapePtr>& shapes,
+                       std::list<EntityInteractionPtr>& ent_inters,
                        const GlobalServicePtr global_services,
-                       const std::set<std::string> &plugin_tags,
+                       const std::set<std::string>& plugin_tags,
                        std::function<void(std::map<std::string, std::string>&)> param_override_func,
                        const int& debug_level) {
     for (std::string ent_inter_name : info.mp->entity_interactions()) {
         ConfigParse config_parse;
-        std::map<std::string, std::string> &overrides =
-            info.mp->attributes()[ent_inter_name];
+        std::map<std::string, std::string>& overrides = info.mp->attributes()[ent_inter_name];
 
         PluginStatus<EntityInteraction> status =
             info.plugin_manager->make_plugin<EntityInteraction>(
-                "scrimmage::EntityInteraction",
-                ent_inter_name, *info.file_search,
-                config_parse, overrides, plugin_tags);
+                "scrimmage::EntityInteraction", ent_inter_name, *info.file_search, config_parse,
+                overrides, plugin_tags);
 
         if (status.status == PluginStatus<EntityInteraction>::cast_failed) {
-            cout << "Failed to load entity interaction plugin: "
-                      << ent_inter_name << endl;
+            cout << "Failed to load entity interaction plugin: " << ent_inter_name << endl;
         } else if (status.status == PluginStatus<EntityInteraction>::loaded) {
             EntityInteractionPtr ent_inter = status.plugin;
 
             // If the name was overridden, use the override.
-            std::string name = get<std::string>("name", config_parse.params(),
-                                                ent_inter_name);
+            std::string name = get<std::string>("name", config_parse.params(), ent_inter_name);
             // Parent specific members
             ent_inter->parent()->set_random(info.random);
             ent_inter->parent()->set_mp(info.mp);
@@ -110,8 +104,7 @@ bool create_ent_inters(const SimUtilsInfo &info,
             ent_inter->init(info.mp->params(), config_parse.params());
 
             // Get shapes from plugin
-            shapes.insert(
-                shapes.end(), ent_inter->shapes().begin(), ent_inter->shapes().end());
+            shapes.insert(shapes.end(), ent_inter->shapes().begin(), ent_inter->shapes().end());
             ent_inter->shapes().clear();
 
             ent_inters.push_back(ent_inter);
@@ -121,23 +114,17 @@ bool create_ent_inters(const SimUtilsInfo &info,
     return true;
 }
 
-bool create_metrics(const SimUtilsInfo &info,
-                    ContactMapPtr contacts,
-                    std::list<MetricsPtr> &metrics_list,
-                    const std::set<std::string> &plugin_tags,
+bool create_metrics(const SimUtilsInfo& info, ContactMapPtr contacts,
+                    std::list<MetricsPtr>& metrics_list, const std::set<std::string>& plugin_tags,
                     std::function<void(std::map<std::string, std::string>&)> param_override_func,
                     const int& debug_level) {
-
     for (std::string metrics_name : info.mp->metrics()) {
         ConfigParse config_parse;
-        std::map<std::string, std::string> &overrides =
-            info.mp->attributes()[metrics_name];
+        std::map<std::string, std::string>& overrides = info.mp->attributes()[metrics_name];
 
-        PluginStatus<Metrics> status =
-            info.plugin_manager->make_plugin<Metrics>(
-                "scrimmage::Metrics", metrics_name,
-                *info.file_search, config_parse, overrides,
-                plugin_tags);
+        PluginStatus<Metrics> status = info.plugin_manager->make_plugin<Metrics>(
+            "scrimmage::Metrics", metrics_name, *info.file_search, config_parse, overrides,
+            plugin_tags);
 
         if (status.status == PluginStatus<Metrics>::cast_failed) {
             cout << "Failed to load metrics: " << metrics_name << endl;
@@ -176,7 +163,7 @@ bool create_metrics(const SimUtilsInfo &info,
 }
 
 void run_callbacks(EntityPluginPtr plugin) {
-    for (auto &sub : plugin->subs()) {
+    for (auto& sub : plugin->subs()) {
         for (auto msg : sub->pop_msgs<MessageBase>()) {
             sub->accept(msg);
         }
@@ -186,11 +173,12 @@ void run_callbacks(EntityPluginPtr plugin) {
 namespace {
 // https://stackoverflow.com/a/48164204
 std::function<void(int)> shutdown_handler;
-void signal_handler(int signal) { shutdown_handler(signal); }
-} // namespace
+void signal_handler(int signal) {
+    shutdown_handler(signal);
+}
+}  // namespace
 
-boost::optional<std::string> run_test(const std::string& mission,
-                                      const bool& init_python,
+boost::optional<std::string> run_test(const std::string& mission, const bool& init_python,
                                       const bool& shutdown_python) {
     SimControl simcontrol;
     if (not simcontrol.init(mission, init_python)) {
@@ -200,8 +188,8 @@ boost::optional<std::string> run_test(const std::string& mission,
 
     // Handle kill signals
     struct sigaction sa;
-    memset( &sa, 0, sizeof(sa) );
-    shutdown_handler = [&](int /*s*/){
+    memset(&sa, 0, sizeof(sa));
+    shutdown_handler = [&](int /*s*/) {
         cout << endl << "Exiting gracefully" << endl;
         simcontrol.force_exit();
     };
@@ -228,30 +216,25 @@ boost::optional<std::string> run_test(const std::string& mission,
     return out;
 }
 
-bool create_networks(const SimUtilsInfo &info, NetworkMap &networks,
-                     const std::set<std::string> &plugin_tags,
+bool create_networks(const SimUtilsInfo& info, NetworkMap& networks,
+                     const std::set<std::string>& plugin_tags,
                      std::function<void(std::map<std::string, std::string>&)> param_override_func,
                      const int& debug_level) {
-
     for (std::string network_name : info.mp->network_names()) {
         ConfigParse config_parse;
-        std::map<std::string, std::string> &overrides =
-            info.mp->attributes()[network_name];
+        std::map<std::string, std::string>& overrides = info.mp->attributes()[network_name];
 
-        PluginStatus<Network> status =
-            info.plugin_manager->make_plugin<Network>(
-                "scrimmage::Network", network_name, *info.file_search,
-                config_parse, overrides, plugin_tags);
+        PluginStatus<Network> status = info.plugin_manager->make_plugin<Network>(
+            "scrimmage::Network", network_name, *info.file_search, config_parse, overrides,
+            plugin_tags);
 
         if (status.status == PluginStatus<Network>::cast_failed) {
-            cout << "Failed to load network plugin: "
-                << network_name << endl;
+            cout << "Failed to load network plugin: " << network_name << endl;
             return false;
         } else if (status.status == PluginStatus<Network>::loaded) {
             NetworkPtr network = status.plugin;
             // If the name was overridden, use the override.
-            std::string name =
-                get<std::string>("name", config_parse.params(), network_name);
+            std::string name = get<std::string>("name", config_parse.params(), network_name);
             network->parent()->set_printer(info.printer);
             network->set_name(name);
             network->set_mission_parse(info.mp);
@@ -279,4 +262,4 @@ bool create_networks(const SimUtilsInfo &info, NetworkMap &networks,
     }
     return true;
 }
-} // namespace scrimmage
+}  // namespace scrimmage
diff --git a/src/viewer/CameraInterface.cpp b/src/viewer/CameraInterface.cpp
index f6038f5006..7aa8f1f4ba 100644
--- a/src/viewer/CameraInterface.cpp
+++ b/src/viewer/CameraInterface.cpp
@@ -30,35 +30,35 @@
  *
  */
 #include <scrimmage/viewer/CameraInterface.h>
-
-#include <vtkWorldPointPicker.h>
 #include <vtkRendererCollection.h>
+#include <vtkWorldPointPicker.h>
 
 namespace scrimmage {
 
-CameraInterface::CameraInterface() {}
+CameraInterface::CameraInterface() {
+}
 
-CameraInterface *CameraInterface::New() {
-    CameraInterface *cb = new CameraInterface;
+CameraInterface* CameraInterface::New() {
+    CameraInterface* cb = new CameraInterface;
     return cb;
 }
 
 void CameraInterface::OnKeyPress() {
     // Get the keypress
-    vtkRenderWindowInteractor *rwi = this->Interactor;
+    vtkRenderWindowInteractor* rwi = this->Interactor;
     std::string key = rwi->GetKeySym();
 
     if (last_key_ == "semicolon") {
-      updater_->process_custom_key(key);
-      last_key_ = "";
+        updater_->process_custom_key(key);
+        last_key_ = "";
     } else if (key == "Right") {
         updater_->inc_follow();
     } else if (key == "Left") {
         updater_->dec_follow();
     } else if (key == "a") {
         updater_->next_mode();
-    } else if (key == "u") { 
-        updater_->undo_camera(); 
+    } else if (key == "u") {
+        updater_->undo_camera();
     } else if (key == "c") {
         updater_->request_cached();
     } else if (key == "t") {
@@ -94,7 +94,7 @@ void CameraInterface::OnKeyPress() {
     } else if (key == "h") {
         updater_->toggle_helpmenu();
     } else if (key == "semicolon") {
-      last_key_ = key;
+        last_key_ = key;
     } else {
         // cout << "key: " << key << endl;
     }
@@ -110,10 +110,10 @@ void CameraInterface::Rotate() {
 
 void CameraInterface::OnLeftButtonDown() {
     if (enable_object_draw_) {
-        this->Interactor->GetPicker()->Pick(this->Interactor->GetEventPosition()[0],
-                                            this->Interactor->GetEventPosition()[1],
-                                            0,  // always zero.
-                                            this->Interactor->GetRenderWindow()->GetRenderers()->GetFirstRenderer());
+        this->Interactor->GetPicker()->Pick(
+            this->Interactor->GetEventPosition()[0], this->Interactor->GetEventPosition()[1],
+            0,  // always zero.
+            this->Interactor->GetRenderWindow()->GetRenderers()->GetFirstRenderer());
         double picked[3];
         this->Interactor->GetPicker()->GetPickPosition(picked);
         updater_->world_point_clicked(picked[0], picked[1], picked[2]);
@@ -141,4 +141,4 @@ void CameraInterface::Dolly() {
     updater_->update();
     vtkInteractorStyleTrackballCamera::Dolly();
 }
-} // namespace scrimmage
+}  // namespace scrimmage
diff --git a/src/viewer/Grid.cpp b/src/viewer/Grid.cpp
index 2733975e6c..3e617b61f7 100644
--- a/src/viewer/Grid.cpp
+++ b/src/viewer/Grid.cpp
@@ -34,13 +34,12 @@
 
 namespace scrimmage {
 
-void Grid::create(int size, double spacing, vtkSmartPointer<vtkRenderer> &renderer) {
+void Grid::create(int size, double spacing, vtkSmartPointer<vtkRenderer>& renderer) {
     renderer_ = renderer;
 
     // Create a plane
     double plane_size = size;
-    vtkSmartPointer<vtkPlaneSource> planeSource =
-            vtkSmartPointer<vtkPlaneSource>::New();
+    vtkSmartPointer<vtkPlaneSource> planeSource = vtkSmartPointer<vtkPlaneSource>::New();
     planeSource->SetCenter(0, 0, 0);
     planeSource->SetNormal(plane_size / 2.0, plane_size / 2.0, 1.0);
     planeSource->SetPoint1(plane_size, 0, 0);
@@ -52,8 +51,7 @@ void Grid::create(int size, double spacing, vtkSmartPointer<vtkRenderer> &render
     const double z_pos = 0;
 
     auto create_line = [&](double val, bool along_y) {
-        vtkSmartPointer<vtkLineSource> lineSource =
-                vtkSmartPointer<vtkLineSource>::New();
+        vtkSmartPointer<vtkLineSource> lineSource = vtkSmartPointer<vtkLineSource>::New();
 
         if (along_y) {
             lineSource->SetPoint1(low, val, z_pos);
@@ -66,12 +64,10 @@ void Grid::create(int size, double spacing, vtkSmartPointer<vtkRenderer> &render
         lineSource->Update();
 
         // Visualize
-        vtkSmartPointer<vtkPolyDataMapper> mapper =
-                vtkSmartPointer<vtkPolyDataMapper>::New();
+        vtkSmartPointer<vtkPolyDataMapper> mapper = vtkSmartPointer<vtkPolyDataMapper>::New();
         mapper->SetInputConnection(lineSource->GetOutputPort());
 
-        vtkSmartPointer<vtkActor> actor =
-                vtkSmartPointer<vtkActor>::New();
+        vtkSmartPointer<vtkActor> actor = vtkSmartPointer<vtkActor>::New();
         actor->SetMapper(mapper);
         actor->GetProperty()->SetLineWidth(1);
 
@@ -83,7 +79,6 @@ void Grid::create(int size, double spacing, vtkSmartPointer<vtkRenderer> &render
         renderer->AddActor(actor);
     };
 
-
     double val = low;
     while (val <= high) {
         create_line(val, true);
@@ -99,4 +94,4 @@ void Grid::remove() {
     actors_.clear();
 }
 
-} // namespace scrimmage
+}  // namespace scrimmage
diff --git a/src/viewer/OriginAxes.cpp b/src/viewer/OriginAxes.cpp
index 029095f058..c0ce19c993 100644
--- a/src/viewer/OriginAxes.cpp
+++ b/src/viewer/OriginAxes.cpp
@@ -34,15 +34,14 @@
 
 namespace scrimmage {
 
-void OriginAxes::create(int length, vtkSmartPointer<vtkRenderer> &renderer) {
+void OriginAxes::create(int length, vtkSmartPointer<vtkRenderer>& renderer) {
     renderer_ = renderer;
 
     axes_ = vtkSmartPointer<vtkAxesActor>::New();
     axes_->SetTotalLength(length, length, length);
 
     // The axes are positioned with a user transform
-    vtkSmartPointer<vtkTransform> transform =
-            vtkSmartPointer<vtkTransform>::New();
+    vtkSmartPointer<vtkTransform> transform = vtkSmartPointer<vtkTransform>::New();
     transform->Translate(0, 0.0, 0);
 
     axes_->SetUserTransform(transform);
@@ -58,4 +57,4 @@ void OriginAxes::remove() {
     renderer_->RemoveActor(axes_);
 }
 
-} // namespace scrimmage
+}  // namespace scrimmage
diff --git a/src/viewer/Updater.cpp b/src/viewer/Updater.cpp
index ce46ea3a9c..18f3f6633d 100644
--- a/src/viewer/Updater.cpp
+++ b/src/viewer/Updater.cpp
@@ -32,65 +32,64 @@
 
 #include <scrimmage/common/FileSearch.h>
 #include <scrimmage/common/Utilities.h>
-#include <scrimmage/math/Quaternion.h>
 #include <scrimmage/math/Angles.h>
+#include <scrimmage/math/Quaternion.h>
 #include <scrimmage/network/Interface.h>
 #include <scrimmage/parse/ConfigParse.h>
 #include <scrimmage/parse/ParseUtils.h>
 #include <scrimmage/proto/ProtoConversions.h>
+#include <scrimmage/viewer/Grid.h>
 #include <scrimmage/viewer/OriginAxes.h>
 #include <scrimmage/viewer/Updater.h>
-#include <scrimmage/viewer/Grid.h>
-
-#include <iostream>
-
-#include <vtkRenderer.h>
-#include <vtkRenderWindow.h>
-#include <vtkRenderWindowInteractor.h>
-#include <vtkDataSetMapper.h>
-#include <vtkPNGReader.h>
-#include <vtkOBJReader.h>
-#include <vtkSTLReader.h>
-#include <vtkTransform.h>
-#include <vtkTransformPolyDataFilter.h>
-#include <vtkTriangle.h>
+#include <vtkArcSource.h>
 #include <vtkArrowSource.h>
-#include <vtkVectorText.h>
+#include <vtkCallbackCommand.h>
 #include <vtkCamera.h>
+#include <vtkCellArray.h>
+#include <vtkConeSource.h>
+#include <vtkCubeSource.h>
+#include <vtkDataSetMapper.h>
 #include <vtkJPEGReader.h>
-#include <vtkPolyDataReader.h>
-#include <vtkSmoothPolyDataFilter.h>
+#include <vtkOBJReader.h>
+#include <vtkPNGReader.h>
+#include <vtkPNGWriter.h>
 #include <vtkParametricFunctionSource.h>
 #include <vtkParametricSpline.h>
-#include <vtkPolyDataNormals.h>
-#include <vtkTextureMapToPlane.h>
+#include <vtkPlaneSource.h>
 #include <vtkPointData.h>
 #include <vtkPoints.h>
-#include <vtkPolygon.h>
+#include <vtkPolyDataNormals.h>
+#include <vtkPolyDataReader.h>
 #include <vtkPolyLine.h>
+#include <vtkPolygon.h>
+#include <vtkPyramid.h>
+#include <vtkRegularPolygonSource.h>
+#include <vtkRenderWindow.h>
+#include <vtkRenderWindowInteractor.h>
+#include <vtkRenderer.h>
+#include <vtkSTLReader.h>
+#include <vtkSmoothPolyDataFilter.h>
+#include <vtkSphereSource.h>
 #include <vtkTextActor.h>
 #include <vtkTextProperty.h>
-#include <vtkCallbackCommand.h>
-#include <vtkCellArray.h>
-#include <vtkSphereSource.h>
-#include <vtkCubeSource.h>
-#include <vtkPyramid.h>
+#include <vtkTextureMapToPlane.h>
+#include <vtkTransform.h>
+#include <vtkTransformPolyDataFilter.h>
+#include <vtkTriangle.h>
 #include <vtkUnstructuredGrid.h>
-#include <vtkConeSource.h>
+#include <vtkVectorText.h>
 #include <vtkVertexGlyphFilter.h>
-#include <vtkPlaneSource.h>
-#include <vtkRegularPolygonSource.h>
 #include <vtkWindowToImageFilter.h>
-#include <vtkPNGWriter.h>
-#include <vtkArcSource.h>
+
+#include <iostream>
 #include <vtksys/SystemTools.hxx>
 
 #if VTK_MAJOR_VERSION > 6
-#include <vtkCellLocator.h>
-#include <vtkGeoJSONReader.h>
+#include <vtkAppendPolyData.h>
 #include <vtkCellData.h>
+#include <vtkCellLocator.h>
 #include <vtkDoubleArray.h>
-#include <vtkAppendPolyData.h>
+#include <vtkGeoJSONReader.h>
 #include <vtkLinearExtrusionFilter.h>
 #include <vtkProperty.h>
 #endif
@@ -109,30 +108,33 @@ namespace sc = scrimmage;
 namespace scrimmage {
 
 double fps = 0;
-void fpsCallbackFunction(vtkObject* caller, long unsigned int vtkNotUsed(eventId), // NOLINT
+void fpsCallbackFunction(vtkObject* caller, long unsigned int vtkNotUsed(eventId),  // NOLINT
                          void* vtkNotUsed(clientData), void* vtkNotUsed(callData)) {
     vtkRenderer* renderer = static_cast<vtkRenderer*>(caller);
     double timeInSeconds = renderer->GetLastRenderTimeInSeconds();
-    fps = 1.0/timeInSeconds;
+    fps = 1.0 / timeInSeconds;
 }
 
-void Updater::set_quat(const scrimmage_proto::Quaternion &quat,
-                       vtkSmartPointer<vtkActor> &actor) {
+void Updater::set_quat(const scrimmage_proto::Quaternion& quat, vtkSmartPointer<vtkActor>& actor) {
     set_quat(proto_2_quat(quat), actor);
 }
 
-void Updater::set_quat(const scrimmage::Quaternion &quat,
-                       vtkSmartPointer<vtkActor> &actor) {
+void Updater::set_quat(const scrimmage::Quaternion& quat, vtkSmartPointer<vtkActor>& actor) {
     Eigen::AngleAxisd aa(quat);
-    actor->SetOrientation(0, 0, 0); // Reset to zero rotation
-    actor->RotateWXYZ(sc::Angles::rad2deg(aa.angle()), aa.axis()(0),
-                      aa.axis()(1), aa.axis()(2));
-}
-
-Updater::Updater() :
-        frame_time_(-1.0), update_count(0), inc_follow_(true),
-        dec_follow_(false), view_mode_(ViewMode::FOLLOW), view_mode_prev_(ViewMode::FREE),
-        follow_offset_(50), follow_vec_(-50, 10, 10), show_helpmenu_(false) {
+    actor->SetOrientation(0, 0, 0);  // Reset to zero rotation
+    actor->RotateWXYZ(sc::Angles::rad2deg(aa.angle()), aa.axis()(0), aa.axis()(1), aa.axis()(2));
+}
+
+Updater::Updater()
+    : frame_time_(-1.0),
+      update_count(0),
+      inc_follow_(true),
+      dec_follow_(false),
+      view_mode_(ViewMode::FOLLOW),
+      view_mode_prev_(ViewMode::FREE),
+      follow_offset_(50),
+      follow_vec_(-50, 10, 10),
+      show_helpmenu_(false) {
     prev_time.tv_nsec = 0;
     prev_time.tv_sec = 0;
     max_update_rate_ = 1.0;
@@ -152,7 +154,7 @@ Updater::Updater() :
     send_shutdown_msg_ = true;
 }
 
-void Updater::init(const std::string &log_dir, double dt) {
+void Updater::init(const std::string& log_dir, double dt) {
     // Create a default grid:
     grid_ = std::make_shared<Grid>();
     grid_->create(20, 1, renderer_);
@@ -168,29 +170,27 @@ void Updater::init(const std::string &log_dir, double dt) {
     dt_ = dt;
 }
 
-void Updater::Execute(vtkObject *caller, unsigned long vtkNotUsed(eventId), // NOLINT
-                      void * vtkNotUsed(callData)) {
+void Updater::Execute(vtkObject* caller, unsigned long vtkNotUsed(eventId),  // NOLINT
+                      void* vtkNotUsed(callData)) {
     if (not update()) {
         return;
     }
 
-    vtkRenderWindowInteractor *iren = vtkRenderWindowInteractor::SafeDownCast(caller);
+    vtkRenderWindowInteractor* iren = vtkRenderWindowInteractor::SafeDownCast(caller);
     iren->GetRenderWindow()->Render();
 
     incoming_interface_->gui_msg_mutex.lock();
-    auto &gui_msg_list = incoming_interface_->gui_msg();
+    auto& gui_msg_list = incoming_interface_->gui_msg();
     while (!gui_msg_list.empty()) {
-
-        auto &msg = gui_msg_list.front();
+        auto& msg = gui_msg_list.front();
         bool time_near = std::abs(msg.time() - frame_time_) < 1e-7;
 
         if (time_near and !fs::exists(log_dir_) and !log_dir_warning_) {
-            std::cout << "Updater cannot save screenshot because the log dir "
-                << log_dir_ << " does not exist" << std::endl;
+            std::cout << "Updater cannot save screenshot because the log dir " << log_dir_
+                      << " does not exist" << std::endl;
             log_dir_warning_ = true;
         }
         if (time_near && fs::exists(log_dir_)) {
-
             vtkSmartPointer<vtkWindowToImageFilter> windowToImageFilter =
                 vtkSmartPointer<vtkWindowToImageFilter>::New();
             windowToImageFilter->SetInput(rwi_->GetRenderWindow());
@@ -205,7 +205,7 @@ void Updater::Execute(vtkObject *caller, unsigned long vtkNotUsed(eventId), // N
             }
             fname = log_dir_ + "/screenshot_" + fname + std::to_string(screenshot_num_) + ".png";
             screenshot_num_++;
-            
+
             if (!fs::exists(fname)) {
                 writer->SetFileName(fname.c_str());
                 writer->SetInputConnection(windowToImageFilter->GetOutputPort());
@@ -227,7 +227,7 @@ bool Updater::update() {
     struct timespec now;
     clock_gettime(CLOCK_MONOTONIC, &now);
     uint64_t diff = BILLION * (now.tv_sec - prev_time.tv_sec) + now.tv_nsec - prev_time.tv_nsec;
-    if (diff < (1.0 / max_update_rate_*BILLION)) {
+    if (diff < (1.0 / max_update_rate_ * BILLION)) {
         return true;
     }
     prev_time = now;
@@ -239,10 +239,10 @@ bool Updater::update() {
     // Do we have any updates to the terrain info
     if (incoming_interface_->utm_terrain_update()) {
         incoming_interface_->utm_terrain_mutex.lock();
-        auto &utms = incoming_interface_->utm_terrain();
+        auto& utms = incoming_interface_->utm_terrain();
 
         // We only care about the last message
-        auto &utm = utms.back();
+        auto& utm = utms.back();
 
         // Process utm terrain data
         update_utm_terrain(utm);
@@ -254,7 +254,7 @@ bool Updater::update() {
     // Do we have any updates to the contact_visuals
     if (incoming_interface_->contact_visual_update()) {
         incoming_interface_->contact_visual_mutex.lock();
-        auto &cv = incoming_interface_->contact_visual();
+        auto& cv = incoming_interface_->contact_visual();
         auto it = cv.begin();
         while (it != cv.end()) {
             (*it)->set_update_required(true);
@@ -267,7 +267,7 @@ bool Updater::update() {
     // Do we have any updates to the frames?
     if (incoming_interface_->frames_update(frame_time_)) {
         incoming_interface_->frames_mutex.lock();
-        auto &frames = incoming_interface_->frames();
+        auto& frames = incoming_interface_->frames();
 
         // Check to see if we have to remove any contacts.
         // Need to check every frame so we don't miss a discrete removal
@@ -284,7 +284,7 @@ bool Updater::update() {
         }
 
         // We only care about the last frame for display purposes
-        auto &frame = frames.back();
+        auto& frame = frames.back();
         update_contacts(frame);
         frames.erase(frames.begin(), std::next(frames.end(), -1));
 
@@ -300,7 +300,7 @@ bool Updater::update() {
     // Do we have any updates to the sim info?
     if (incoming_interface_->sim_info_update()) {
         incoming_interface_->sim_info_mutex.lock();
-        auto &info_list = incoming_interface_->sim_info();
+        auto& info_list = incoming_interface_->sim_info();
 
         // Check for shutting_down message from simcontrol
         for (auto it : info_list) {
@@ -322,7 +322,7 @@ bool Updater::update() {
     // Do we have any new shapes?
     if (incoming_interface_->shapes_update()) {
         incoming_interface_->shapes_mutex.lock();
-        auto &shapes_list = incoming_interface_->shapes();
+        auto& shapes_list = incoming_interface_->shapes();
 
         auto it = shapes_list.begin();
         while (it != shapes_list.end()) {
@@ -354,7 +354,7 @@ bool Updater::update() {
 }
 
 bool Updater::update_scale() {
-    for (auto &kv : actor_contacts_) {
+    for (auto& kv : actor_contacts_) {
         // Get the desired scale amount, which is a factor of the current
         // visualization scale (scale_) and the scale amount associated with
         // the contact visual information
@@ -365,7 +365,7 @@ bool Updater::update_scale() {
         }
 
         // Get the 4x4 matrix that defines the scale, rotation, and translation
-        vtkMatrix4x4 *m;
+        vtkMatrix4x4* m;
         m = kv.second->actor->GetMatrix();
 
         // Compute the current scale factor for the object. The top-left 3x3
@@ -400,27 +400,27 @@ bool Updater::update_shapes() {
     for (auto it = shapes_.begin(); it != shapes_.end();) {
         bool remove = false;
 
-        scrimmage_proto::Shape &s = std::get<0>(it->second);
+        scrimmage_proto::Shape& s = std::get<0>(it->second);
         if (not s.persistent()) {
             switch (s.oneof_ttl_case()) {
-            case sp::Shape::kTtl:
-                s.set_ttl(s.ttl()-1);
-                if (s.ttl() <= 0) {
-                    remove = true;
-                }
-                break;
-            case sp::Shape::kPersistDuration:
-                if (std::get<3>(it->second) + s.persist_duration() < frame_time_) {
-                    remove = true;
-                }
-                break;
-            case sp::Shape::kPersistUntil:
-                if (s.persist_until() < frame_time_) {
-                    remove = true;
-                }
-                break;
-            default:
-                break;
+                case sp::Shape::kTtl:
+                    s.set_ttl(s.ttl() - 1);
+                    if (s.ttl() <= 0) {
+                        remove = true;
+                    }
+                    break;
+                case sp::Shape::kPersistDuration:
+                    if (std::get<3>(it->second) + s.persist_duration() < frame_time_) {
+                        remove = true;
+                    }
+                    break;
+                case sp::Shape::kPersistUntil:
+                    if (s.persist_until() < frame_time_) {
+                        remove = true;
+                    }
+                    break;
+                default:
+                    break;
             }
         }
 
@@ -434,7 +434,7 @@ bool Updater::update_shapes() {
     return true;
 }
 
-bool Updater::draw_shapes(scrimmage_proto::Shapes &shapes) {
+bool Updater::draw_shapes(scrimmage_proto::Shapes& shapes) {
     // Display new shapes
     for (int i = 0; i < shapes.shape_size(); i++) {
         sp::Shape shape = shapes.shape(i);
@@ -458,59 +458,59 @@ bool Updater::draw_shapes(scrimmage_proto::Shapes &shapes) {
 
         bool shape_status = false;
         switch (shape.oneof_type_case()) {
-        case sp::Shape::kArc:
-            shape_status = draw_arc(new_shape, shape.arc(), actor, source, mapper);
-            break;
-        case sp::Shape::kArrow:
-            shape_status = draw_arrow(new_shape, shape.arrow(), actor, source, mapper);
-            break;
-        case sp::Shape::kCircle:
-            shape_status = draw_circle(new_shape, shape.circle(), actor, source, mapper);
-            break;
-        case sp::Shape::kCone:
-            shape_status = draw_cone(new_shape, shape.cone(), actor, source, mapper);
-            break;
-        case sp::Shape::kCuboid:
-            shape_status = draw_cube(new_shape, shape.cuboid(), actor, source, mapper);
-            break;
-        case sp::Shape::kEllipse:
-            shape_status = draw_ellipse(new_shape, shape.ellipse(), actor, source, mapper);
-            break;
-        case sp::Shape::kLine:
-            shape_status = draw_line(new_shape, shape.line(), actor, source, mapper);
-            break;
-        case sp::Shape::kMesh:
-            shape_status = draw_mesh(new_shape, shape.mesh(), actor, source, mapper);
-            break;
-        case sp::Shape::kPlane:
-            shape_status = draw_plane(new_shape, shape.plane(), actor, source, mapper);
-            break;
-        case sp::Shape::kPointcloud:
-            shape_status = draw_pointcloud(new_shape, shape, actor, source, mapper);
-            break;
-        case sp::Shape::kPolydata:
-            shape_status = draw_polydata(new_shape, shape.polydata(), actor, source, mapper);
-            break;
-        case sp::Shape::kPolygon:
-            shape_status = draw_polygon(new_shape, shape.polygon(), actor, source, mapper);
-            break;
-        case sp::Shape::kPolyline:
-            shape_status = draw_polyline(new_shape, shape.polyline(), actor, source, mapper);
-            break;
-        case sp::Shape::kSphere:
-            shape_status = draw_sphere(new_shape, shape.sphere(), actor, source, mapper);
-            break;
-        case sp::Shape::kSpline:
-            shape_status = draw_spline(new_shape, shape.spline(), actor, source, mapper);
-            break;
-        case sp::Shape::kText:
-            shape_status = draw_text(new_shape, shape.text(), actor, source, mapper);
-            break;
-        case sp::Shape::kTriangle:
-            shape_status = draw_triangle(new_shape, shape.triangle(), actor, source, mapper);
-            break;
-        default:
-            break;
+            case sp::Shape::kArc:
+                shape_status = draw_arc(new_shape, shape.arc(), actor, source, mapper);
+                break;
+            case sp::Shape::kArrow:
+                shape_status = draw_arrow(new_shape, shape.arrow(), actor, source, mapper);
+                break;
+            case sp::Shape::kCircle:
+                shape_status = draw_circle(new_shape, shape.circle(), actor, source, mapper);
+                break;
+            case sp::Shape::kCone:
+                shape_status = draw_cone(new_shape, shape.cone(), actor, source, mapper);
+                break;
+            case sp::Shape::kCuboid:
+                shape_status = draw_cube(new_shape, shape.cuboid(), actor, source, mapper);
+                break;
+            case sp::Shape::kEllipse:
+                shape_status = draw_ellipse(new_shape, shape.ellipse(), actor, source, mapper);
+                break;
+            case sp::Shape::kLine:
+                shape_status = draw_line(new_shape, shape.line(), actor, source, mapper);
+                break;
+            case sp::Shape::kMesh:
+                shape_status = draw_mesh(new_shape, shape.mesh(), actor, source, mapper);
+                break;
+            case sp::Shape::kPlane:
+                shape_status = draw_plane(new_shape, shape.plane(), actor, source, mapper);
+                break;
+            case sp::Shape::kPointcloud:
+                shape_status = draw_pointcloud(new_shape, shape, actor, source, mapper);
+                break;
+            case sp::Shape::kPolydata:
+                shape_status = draw_polydata(new_shape, shape.polydata(), actor, source, mapper);
+                break;
+            case sp::Shape::kPolygon:
+                shape_status = draw_polygon(new_shape, shape.polygon(), actor, source, mapper);
+                break;
+            case sp::Shape::kPolyline:
+                shape_status = draw_polyline(new_shape, shape.polyline(), actor, source, mapper);
+                break;
+            case sp::Shape::kSphere:
+                shape_status = draw_sphere(new_shape, shape.sphere(), actor, source, mapper);
+                break;
+            case sp::Shape::kSpline:
+                shape_status = draw_spline(new_shape, shape.spline(), actor, source, mapper);
+                break;
+            case sp::Shape::kText:
+                shape_status = draw_text(new_shape, shape.text(), actor, source, mapper);
+                break;
+            case sp::Shape::kTriangle:
+                shape_status = draw_triangle(new_shape, shape.triangle(), actor, source, mapper);
+                break;
+            default:
+                break;
         }
 
         // Only add the actor if it was correctly constructed
@@ -522,14 +522,12 @@ bool Updater::draw_shapes(scrimmage_proto::Shapes &shapes) {
                 actor->GetProperty()->SetOpacity(shape.opacity());
             }
 
-            actor->GetProperty()->SetColor(shape.color().r()/255.0,
-                                           shape.color().g()/255.0,
-                                           shape.color().b()/255.0);
+            actor->GetProperty()->SetColor(shape.color().r() / 255.0, shape.color().g() / 255.0,
+                                           shape.color().b() / 255.0);
 
             if (new_shape) {
                 renderer_->AddActor(actor);
-                shapes_[shape.hash()] = std::make_tuple(shape, actor, source,
-                                                        frame_time_);
+                shapes_[shape.hash()] = std::make_tuple(shape, actor, source, frame_time_);
             } else {
                 std::get<0>(it->second) = shape;
             }
@@ -604,7 +602,7 @@ bool Updater::update_camera() {
     double y_pos_fp = y_pos;
     double z_pos_fp = z_pos;
 
-    double camera_pos[3] {0, 0, 0};
+    double camera_pos[3]{0, 0, 0};
     if (view_mode_ == ViewMode::FREE) {
         if (reset_camera_) {
             x_pos_fp = camera_reset_params_.focal_x;
@@ -628,14 +626,13 @@ bool Updater::update_camera() {
             camera_pos[1] = y_pos - 6.0;
             camera_pos[2] = z_pos + 2.0;
         } else if (view_mode_ == ViewMode::FOLLOW) {
-
             if (view_mode_prev_ == ViewMode::FOLLOW) {
                 double currentPos[3];
                 renderer_->GetActiveCamera()->GetPosition(currentPos);
                 double currentFp[3];
                 renderer_->GetActiveCamera()->GetFocalPoint(currentFp);
                 for (int i = 0; i < 3; i++)
-                    follow_vec_[i] = currentPos[i]-currentFp[i];
+                    follow_vec_[i] = currentPos[i] - currentFp[i];
                 follow_offset_ = follow_vec_.norm();
             }
 
@@ -645,8 +642,8 @@ bool Updater::update_camera() {
             sp::Quaternion sp_quat = it->second->contact.state().orientation();
             sc::Quaternion quat(sp_quat.w(), sp_quat.x(), sp_quat.y(), sp_quat.z());
 
-            Eigen::Vector3d pos = Eigen::Vector3d(x_pos, y_pos, z_pos) +
-                unit_vector * rel_cam_pos.norm();
+            Eigen::Vector3d pos =
+                Eigen::Vector3d(x_pos, y_pos, z_pos) + unit_vector * rel_cam_pos.norm();
 
             camera_pos[0] = pos[0];
             camera_pos[1] = pos[1];
@@ -662,8 +659,7 @@ bool Updater::update_camera() {
 
             // Compute camera position
             Eigen::Vector3d base_offset(1, 0, 0);
-            Eigen::Vector3d pos = Eigen::Vector3d(x_pos, y_pos, z_pos) +
-                quat * base_offset * 5;
+            Eigen::Vector3d pos = Eigen::Vector3d(x_pos, y_pos, z_pos) + quat * base_offset * 5;
 
             // Compute camera focal point
             Eigen::Vector3d cam_dir = quat.rotate(base_offset.normalized());
@@ -687,7 +683,7 @@ bool Updater::update_camera() {
     reset_camera_ = false;
     renderer_->GetActiveCamera()->SetPosition(camera_pos);
     renderer_->GetActiveCamera()->SetFocalPoint(x_pos_fp, y_pos_fp, z_pos_fp);
-    renderer_->ResetCameraClippingRange(); // fixes missing terrain/entity issue
+    renderer_->ResetCameraClippingRange();  // fixes missing terrain/entity issue
     return true;
 }
 
@@ -727,7 +723,8 @@ bool Updater::update_text_display() {
             std::string heading_str = "H: " + std::to_string(sc::Angles::rad2deg(quat.yaw()));
             heading_actor_->SetInput(heading_str.c_str());
 
-            std::string alt_str = "Alt: " + std::to_string(it->second->contact.state().position().z());
+            std::string alt_str =
+                "Alt: " + std::to_string(it->second->contact.state().position().z());
             alt_actor_->SetInput(alt_str.c_str());
         }
     }
@@ -735,7 +732,6 @@ bool Updater::update_text_display() {
     return true;
 }
 
-
 void Updater::next_mode() {
     switch (view_mode_) {
         case ViewMode::FOLLOW:
@@ -756,9 +752,9 @@ void Updater::next_mode() {
     }
 }
 
-void Updater::track_camera_pos() { 
+void Updater::track_camera_pos() {
     // Camera cannot be updated for OFFSET or FPV view modes
-    if((view_mode_ == ViewMode::OFFSET) || (view_mode_ == ViewMode::FPV)) {
+    if ((view_mode_ == ViewMode::OFFSET) || (view_mode_ == ViewMode::FPV)) {
         return;
     }
 
@@ -768,15 +764,15 @@ void Updater::track_camera_pos() {
 
     double currentFp[3];
     renderer_->GetActiveCamera()->GetFocalPoint(currentFp);
-        
-    double camera_pos[3] {0, 0, 0};
+
+    double camera_pos[3]{0, 0, 0};
     camera_pos[0] = currentPos[0];
     camera_pos[1] = currentPos[1];
     camera_pos[2] = currentPos[2];
 
-    double x_pos_fp = currentFp[0];  
-    double y_pos_fp = currentFp[1]; 
-    double z_pos_fp = currentFp[2]; 
+    double x_pos_fp = currentFp[0];
+    double y_pos_fp = currentFp[1];
+    double z_pos_fp = currentFp[2];
 
     std::vector<double> camera_pos_tracker;
 
@@ -794,7 +790,8 @@ void Updater::track_camera_pos() {
 void Updater::undo_camera() {
     // Return with no update if the view modes are OFFSET or FPV or if there
     // is no previous state to return to
-    if((view_mode_ == ViewMode::OFFSET) || (view_mode_ == ViewMode::FPV) || prev_camera_pos.size() < 2) {
+    if ((view_mode_ == ViewMode::OFFSET) || (view_mode_ == ViewMode::FPV)
+        || prev_camera_pos.size() < 2) {
         return;
     } else {
         std::vector<double> new_camera_pos;
@@ -813,7 +810,7 @@ void Updater::undo_camera() {
     }
 }
 
-bool Updater::update_utm_terrain(std::shared_ptr<scrimmage_proto::UTMTerrain> &utm) {
+bool Updater::update_utm_terrain(std::shared_ptr<scrimmage_proto::UTMTerrain>& utm) {
     // Reset the grid
     grid_->remove();
     if (utm->enable_grid()) {
@@ -827,8 +824,7 @@ bool Updater::update_utm_terrain(std::shared_ptr<scrimmage_proto::UTMTerrain> &u
     }
 
     // Set the background color:
-    renderer_->SetBackground(utm->background().r() / 255.0,
-                             utm->background().g() / 255.0,
+    renderer_->SetBackground(utm->background().r() / 255.0, utm->background().g() / 255.0,
                              utm->background().b() / 255.0);
 
     // Exit if the terrain is disabled in this messasge
@@ -841,21 +837,18 @@ bool Updater::update_utm_terrain(std::shared_ptr<scrimmage_proto::UTMTerrain> &u
     } else {
         // Search for the appropriate files on the local system
         ConfigParse terrain_parse;
-        find_terrain_files(utm->terrain_name(),
-                           terrain_parse, utm);
+        find_terrain_files(utm->terrain_name(), terrain_parse, utm);
         terrain_map_[utm->terrain_name()] = utm;
     }
 
     if (utm->enable_terrain()) {
         // Read and create texture...
-        vtkSmartPointer<vtkJPEGReader> terrain_jPEGReader =
-            vtkSmartPointer<vtkJPEGReader>::New();
+        vtkSmartPointer<vtkJPEGReader> terrain_jPEGReader = vtkSmartPointer<vtkJPEGReader>::New();
         terrain_jPEGReader->SetFileName(utm->texture_file().c_str());
         terrain_jPEGReader->Update();
 
         // Apply the texture
-        vtkSmartPointer<vtkTexture> terrain_texture =
-            vtkSmartPointer<vtkTexture>::New();
+        vtkSmartPointer<vtkTexture> terrain_texture = vtkSmartPointer<vtkTexture>::New();
         terrain_texture->SetInputConnection(terrain_jPEGReader->GetOutputPort());
         terrain_texture->InterpolateOn();
 
@@ -891,8 +884,7 @@ bool Updater::update_utm_terrain(std::shared_ptr<scrimmage_proto::UTMTerrain> &u
         }
 
         // Setup colors
-        vtkSmartPointer<vtkUnsignedCharArray> colors =
-            vtkSmartPointer<vtkUnsignedCharArray>::New();
+        vtkSmartPointer<vtkUnsignedCharArray> colors = vtkSmartPointer<vtkUnsignedCharArray>::New();
         colors->SetNumberOfComponents(3);
         colors->SetName("Colors");
         for (int i = 0; i < polydata->GetNumberOfPoints(); ++i) {
@@ -907,10 +899,8 @@ bool Updater::update_utm_terrain(std::shared_ptr<scrimmage_proto::UTMTerrain> &u
         terrain_reader1->Update();
         polydata->GetPointData()->SetScalars(colors);
 
-        vtkSmartPointer<vtkTransform> translation =
-            vtkSmartPointer<vtkTransform>::New();
-        translation->Translate(-utm->x_translate(), -utm->y_translate(),
-                               -utm->z_translate());
+        vtkSmartPointer<vtkTransform> translation = vtkSmartPointer<vtkTransform>::New();
+        translation->Translate(-utm->x_translate(), -utm->y_translate(), -utm->z_translate());
 
         vtkSmartPointer<vtkTransformPolyDataFilter> transformFilter =
             vtkSmartPointer<vtkTransformPolyDataFilter>::New();
@@ -930,7 +920,8 @@ bool Updater::update_utm_terrain(std::shared_ptr<scrimmage_proto::UTMTerrain> &u
         smoothFilter->Update();
 
         // Update normals on newly smoothed polydata
-        vtkSmartPointer<vtkPolyDataNormals> normalGenerator = vtkSmartPointer<vtkPolyDataNormals>::New();
+        vtkSmartPointer<vtkPolyDataNormals> normalGenerator =
+            vtkSmartPointer<vtkPolyDataNormals>::New();
         normalGenerator->SetInputConnection(smoothFilter->GetOutputPort());
         normalGenerator->ComputePointNormalsOn();
         normalGenerator->ComputeCellNormalsOn();
@@ -943,8 +934,7 @@ bool Updater::update_utm_terrain(std::shared_ptr<scrimmage_proto::UTMTerrain> &u
 #if VTK_MAJOR_VERSION > 6
         if (utm->enable_extrusion()) {
             // Locate elevation data
-            vtkSmartPointer<vtkCellLocator> cellLocator =
-              vtkSmartPointer<vtkCellLocator>::New();
+            vtkSmartPointer<vtkCellLocator> cellLocator = vtkSmartPointer<vtkCellLocator>::New();
             cellLocator->SetDataSet(normalGenerator->GetOutput());
             cellLocator->BuildLocator();
             double closestPoint[3];
@@ -957,55 +947,49 @@ bool Updater::update_utm_terrain(std::shared_ptr<scrimmage_proto::UTMTerrain> &u
 
             // Read extrusion file and extract polydata and height property
             vtkSmartPointer<vtkGeoJSONReader> extrusion_reader =
-                    vtkSmartPointer<vtkGeoJSONReader>::New();
+                vtkSmartPointer<vtkGeoJSONReader>::New();
             extrusion_reader->SetFileName(utm->extrusion_file().c_str());
-            extrusion_reader->AddFeatureProperty(
-                    utm->extrusion_property().c_str(), defval);
+            extrusion_reader->AddFeatureProperty(utm->extrusion_property().c_str(), defval);
             extrusion_reader->Update();
 
             vtkSmartPointer<vtkTransformPolyDataFilter> extrudeTransform =
                 vtkSmartPointer<vtkTransformPolyDataFilter>::New();
-            extrudeTransform->SetInputConnection(
-                    extrusion_reader->GetOutputPort());
+            extrudeTransform->SetInputConnection(extrusion_reader->GetOutputPort());
             extrudeTransform->SetTransform(translation);
             extrudeTransform->Update();
 
             // Get all the data in the extrusion file
-            vtkSmartPointer<vtkPolyData> extrusion_data =
-                    vtkSmartPointer<vtkPolyData>::New();
+            vtkSmartPointer<vtkPolyData> extrusion_data = vtkSmartPointer<vtkPolyData>::New();
             extrusion_data = extrudeTransform->GetOutput();
 
             vtkSmartPointer<vtkDoubleArray> heights =
-                    vtkDoubleArray::SafeDownCast(
-                            extrusion_data->GetCellData()->GetArray("Height"));
+                vtkDoubleArray::SafeDownCast(extrusion_data->GetCellData()->GetArray("Height"));
 
             // Extract each polygon, extrude it, and append to new polydata
             vtkIdType numCells = extrusion_data->GetPolys()->GetNumberOfCells();
             vtkSmartPointer<vtkAppendPolyData> appendPolys =
-                        vtkSmartPointer<vtkAppendPolyData>::New();
+                vtkSmartPointer<vtkAppendPolyData>::New();
             for (vtkIdType n(0); n < numCells; n++) {
                 auto pts = vtkSmartPointer<vtkPoints>::New();
                 auto polygon = vtkSmartPointer<vtkPolygon>::New();
                 auto polygons = vtkSmartPointer<vtkCellArray>::New();
                 auto cellPointIds = vtkSmartPointer<vtkIdList>::New();
                 auto polygonPolydata = vtkSmartPointer<vtkPolyData>::New();
-                auto extrudeFilter =
-                        vtkSmartPointer<vtkLinearExtrusionFilter>::New();
+                auto extrudeFilter = vtkSmartPointer<vtkLinearExtrusionFilter>::New();
 
                 // Get PointIds for the n-th polygon in the polydata
                 extrusion_data->GetCellPoints(n, cellPointIds);
 
                 // Set number of ids in the new polygon
-                polygon->GetPointIds()->SetNumberOfIds(
-                        cellPointIds->GetNumberOfIds());
+                polygon->GetPointIds()->SetNumberOfIds(cellPointIds->GetNumberOfIds());
 
                 // Copy the points to the new polygon
                 for (vtkIdType m(0); m < cellPointIds->GetNumberOfIds(); m++) {
                     double p[3];
                     extrusion_data->GetPoint(cellPointIds->GetId(m), p);
                     if (m == 0) {
-                        cellLocator->FindClosestPoint(p, closestPoint, cellId,
-                                subId, closestPointDist2);
+                        cellLocator->FindClosestPoint(p, closestPoint, cellId, subId,
+                                                      closestPointDist2);
                     }
                     pts->InsertNextPoint(p[0], p[1], closestPoint[2]);
                     polygon->GetPointIds()->SetId(m, m);
@@ -1027,11 +1011,11 @@ bool Updater::update_utm_terrain(std::shared_ptr<scrimmage_proto::UTMTerrain> &u
                 appendPolys->AddInputData(extrudeFilter->GetOutput());
             }
             vtkSmartPointer<vtkPolyDataMapper> extrusion_mapper =
-                    vtkSmartPointer<vtkPolyDataMapper>::New();
+                vtkSmartPointer<vtkPolyDataMapper>::New();
             extrusion_mapper->SetInputConnection(appendPolys->GetOutputPort());
 
             if (extrusion_actor_ != NULL) {
-                renderer_ ->RemoveActor(extrusion_actor_);
+                renderer_->RemoveActor(extrusion_actor_);
             }
 
             extrusion_actor_ = vtkSmartPointer<vtkActor>::New();
@@ -1053,17 +1037,17 @@ bool Updater::update_utm_terrain(std::shared_ptr<scrimmage_proto::UTMTerrain> &u
 
         renderer_->AddActor(terrain_actor_);
 #if VTK_MAJOR_VERSION > 6
-        if (utm->enable_extrusion())
-            renderer_->AddActor(extrusion_actor_);
+        if (utm->enable_extrusion()) renderer_->AddActor(extrusion_actor_);
 #endif
     }
     return true;
 }
 
-void Updater::set_max_update_rate(double max_update_rate)
-{ max_update_rate_ = max_update_rate; }
+void Updater::set_max_update_rate(double max_update_rate) {
+    max_update_rate_ = max_update_rate;
+}
 
-void Updater::set_renderer(vtkSmartPointer<vtkRenderer> &renderer) {
+void Updater::set_renderer(vtkSmartPointer<vtkRenderer>& renderer) {
     renderer_ = renderer;
 
     // After the renderer is set, we can setup the text display
@@ -1071,40 +1055,39 @@ void Updater::set_renderer(vtkSmartPointer<vtkRenderer> &renderer) {
     set_view_mode(ViewMode::FOLLOW);
 }
 
-void Updater::set_rwi(vtkSmartPointer<vtkRenderWindowInteractor> &rwi)
-{ rwi_ = rwi; }
+void Updater::set_rwi(vtkSmartPointer<vtkRenderWindowInteractor>& rwi) {
+    rwi_ = rwi;
+}
 
-void Updater::set_incoming_interface(InterfacePtr &incoming_interface)
-{ incoming_interface_ = incoming_interface; }
+void Updater::set_incoming_interface(InterfacePtr& incoming_interface) {
+    incoming_interface_ = incoming_interface;
+}
 
-void Updater::set_outgoing_interface(InterfacePtr &outgoing_interface)
-{ outgoing_interface_ = outgoing_interface; }
+void Updater::set_outgoing_interface(InterfacePtr& outgoing_interface) {
+    outgoing_interface_ = outgoing_interface;
+}
 
-bool Updater::update_contacts(std::shared_ptr<scrimmage_proto::Frame> &frame) {
+bool Updater::update_contacts(std::shared_ptr<scrimmage_proto::Frame>& frame) {
     frame_time_ = frame->time();
 
     // Add new contacts to contact map
     for (int i = 0; i < frame->contact_size(); i++) {
-
         const sp::Contact cnt = frame->contact(i);
         int id = cnt.id().id();
 
         if (actor_contacts_.count(id) == 0 && cnt.active()) {
             // Initialize everything as a sphere until it can be matched
             // with information in the contact_visuals_ map
-            vtkSmartPointer<vtkSphereSource> sphereSource =
-                vtkSmartPointer<vtkSphereSource>::New();
+            vtkSmartPointer<vtkSphereSource> sphereSource = vtkSmartPointer<vtkSphereSource>::New();
             sphereSource->SetCenter(0, 0, 0);
             sphereSource->SetRadius(1);
 
             // Create a mapper
-            vtkSmartPointer<vtkDataSetMapper> mapper =
-                vtkSmartPointer<vtkDataSetMapper>::New();
+            vtkSmartPointer<vtkDataSetMapper> mapper = vtkSmartPointer<vtkDataSetMapper>::New();
             mapper->SetInputConnection(sphereSource->GetOutputPort());
 
             // Create an actor
-            vtkSmartPointer<vtkActor> actor =
-                vtkSmartPointer<vtkActor>::New();
+            vtkSmartPointer<vtkActor> actor = vtkSmartPointer<vtkActor>::New();
             actor->SetMapper(mapper);
 
             actor->GetProperty()->SetColor(161, 161, 161);
@@ -1113,8 +1096,7 @@ bool Updater::update_contacts(std::shared_ptr<scrimmage_proto::Frame> &frame) {
             renderer_->AddActor(actor);
 
             // Add the object label
-            vtkSmartPointer<vtkVectorText> textSource =
-                vtkSmartPointer<vtkVectorText>::New();
+            vtkSmartPointer<vtkVectorText> textSource = vtkSmartPointer<vtkVectorText>::New();
             textSource->SetText(std::to_string(id).c_str());
 
             // Create a mapper for the label
@@ -1123,10 +1105,9 @@ bool Updater::update_contacts(std::shared_ptr<scrimmage_proto::Frame> &frame) {
             label_mapper->SetInputConnection(textSource->GetOutputPort());
 
             // Create a subclass of vtkActor: a vtkFollower that remains facing the camera
-            vtkSmartPointer<vtkFollower> label =
-                vtkSmartPointer<vtkFollower>::New();
+            vtkSmartPointer<vtkFollower> label = vtkSmartPointer<vtkFollower>::New();
             label->SetMapper(label_mapper);
-            label->GetProperty()->SetColor(1, 1, 1); // white
+            label->GetProperty()->SetColor(1, 1, 1);  // white
             label->SetScale(label_scale_, label_scale_, label_scale_);
 
             // Add the actor to the scene
@@ -1180,12 +1161,12 @@ bool Updater::update_contacts(std::shared_ptr<scrimmage_proto::Frame> &frame) {
                 desired_scale_amount = it_cv->second->scale() * scale_;
             }
 
-            vtkMatrix4x4 *m;
+            vtkMatrix4x4* m;
             m = ac->actor->GetMatrix();
 
             for (int r = 0; r < 3; r++) {
                 for (int c = 0; c < 3; c++) {
-                    m->SetElement(r, c, desired_scale_amount*mat3(r, c));
+                    m->SetElement(r, c, desired_scale_amount * mat3(r, c));
                 }
             }
 
@@ -1198,7 +1179,7 @@ bool Updater::update_contacts(std::shared_ptr<scrimmage_proto::Frame> &frame) {
     }
 
     // Set opacity of stale actors
-    for (auto &kv : actor_contacts_) {
+    for (auto& kv : actor_contacts_) {
         if (!kv.second->exists) {
             kv.second->actor->GetProperty()->SetOpacity(0.10);
             kv.second->label->GetProperty()->SetOpacity(0.10);
@@ -1215,11 +1196,12 @@ bool Updater::update_contacts(std::shared_ptr<scrimmage_proto::Frame> &frame) {
             renderer_->RemoveActor(it->second->label);
 
             // Remove the trail points
-            for (std::list<vtkSmartPointer<vtkActor> >::iterator it_trail = it->second->trail.begin();
+            for (std::list<vtkSmartPointer<vtkActor> >::iterator it_trail =
+                     it->second->trail.begin();
                  it_trail != it->second->trail.end(); ++it_trail) {
                 renderer_->RemoveActor(*it_trail);
             }
-            actor_contacts_.erase(it++); // remove map entry
+            actor_contacts_.erase(it++);  // remove map entry
         } else {
             it++;
         }
@@ -1228,8 +1210,8 @@ bool Updater::update_contacts(std::shared_ptr<scrimmage_proto::Frame> &frame) {
     return true;
 }
 
-void Updater::update_contact_visual(std::shared_ptr<ActorContact> &actor_contact,
-                                    std::shared_ptr<scrimmage_proto::ContactVisual> &cv) {
+void Updater::update_contact_visual(std::shared_ptr<ActorContact>& actor_contact,
+                                    std::shared_ptr<scrimmage_proto::ContactVisual>& cv) {
     // Only update the meshes if the model name has changed
     if (actor_contact->model_name != cv->name()) {
         actor_contact->model_name = cv->name();
@@ -1238,8 +1220,7 @@ void Updater::update_contact_visual(std::shared_ptr<ActorContact> &actor_contact
         renderer_->RemoveActor(actor_contact->actor);
 
         // Create a new actor and mapper
-        vtkSmartPointer<vtkDataSetMapper> mapper =
-            vtkSmartPointer<vtkDataSetMapper>::New();
+        vtkSmartPointer<vtkDataSetMapper> mapper = vtkSmartPointer<vtkDataSetMapper>::New();
 
         vtkSmartPointer<vtkActor> actor = vtkSmartPointer<vtkActor>::New();
 
@@ -1251,22 +1232,20 @@ void Updater::update_contact_visual(std::shared_ptr<ActorContact> &actor_contact
         // Setup the overrides based on the data in the message
         std::map<std::string, std::string> overrides;
         overrides["visual_scale"] = std::to_string(cv->scale());
-        overrides["visual_rpy"] = std::to_string(cv->rotate(0)) + " " +
-            std::to_string(cv->rotate(1)) + " " +
-            std::to_string(cv->rotate(2));
+        overrides["visual_rpy"] = std::to_string(cv->rotate(0)) + " "
+                                  + std::to_string(cv->rotate(1)) + " "
+                                  + std::to_string(cv->rotate(2));
 
-        find_model_properties(cv->name(), cv_parse, file_search, overrides,
-                              cv, mesh_found, texture_found);
+        find_model_properties(cv->name(), cv_parse, file_search, overrides, cv, mesh_found,
+                              texture_found);
 
         if (actor_contact->contact.type() == sp::MESH) {
             if (texture_found) {
-                vtkSmartPointer<vtkPNGReader> pngReader =
-                    vtkSmartPointer<vtkPNGReader>::New();
+                vtkSmartPointer<vtkPNGReader> pngReader = vtkSmartPointer<vtkPNGReader>::New();
                 pngReader->SetFileName(cv->texture_file().c_str());
                 pngReader->Update();
 
-                vtkSmartPointer<vtkTexture> colorTexture =
-                    vtkSmartPointer<vtkTexture>::New();
+                vtkSmartPointer<vtkTexture> colorTexture = vtkSmartPointer<vtkTexture>::New();
                 colorTexture->SetInputConnection(pngReader->GetOutputPort());
                 colorTexture->InterpolateOn();
                 actor->SetTexture(colorTexture);
@@ -1284,16 +1263,15 @@ void Updater::update_contact_visual(std::shared_ptr<ActorContact> &actor_contact
                 vtkSmartPointer<vtkTransformPolyDataFilter>::New();
 
             transformFilter->SetTransform(transform);
-            std::string extension = vtksys::SystemTools::GetFilenameLastExtension(std::string(cv->model_file().c_str()));
+            std::string extension = vtksys::SystemTools::GetFilenameLastExtension(
+                std::string(cv->model_file().c_str()));
             if (extension == ".obj") {
-                vtkSmartPointer<vtkOBJReader> reader =
-                    vtkSmartPointer<vtkOBJReader>::New();
+                vtkSmartPointer<vtkOBJReader> reader = vtkSmartPointer<vtkOBJReader>::New();
                 reader->SetFileName(cv->model_file().c_str());
                 reader->Update();
                 transformFilter->SetInputConnection(reader->GetOutputPort());
             } else if (extension == ".stl") {
-                vtkSmartPointer<vtkSTLReader> reader =
-                        vtkSmartPointer<vtkSTLReader>::New();
+                vtkSmartPointer<vtkSTLReader> reader = vtkSmartPointer<vtkSTLReader>::New();
                 reader->SetFileName(cv->model_file().c_str());
                 reader->Update();
                 transformFilter->SetInputConnection(reader->GetOutputPort());
@@ -1307,19 +1285,17 @@ void Updater::update_contact_visual(std::shared_ptr<ActorContact> &actor_contact
             for (int i = 0; i < 3; i++) {
                 scale_data[i] = cv->scale() * scale_;
             }
-            actor->SetScale(scale_data[0], scale_data[1],
-                            scale_data[2]);
+            actor->SetScale(scale_data[0], scale_data[1], scale_data[2]);
         } else if (actor_contact->contact.type() == sp::AIRCRAFT) {
-            vtkSmartPointer<vtkPoints> points =
-                vtkSmartPointer<vtkPoints>::New();
+            vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New();
 
             float size = 2.0;
-            float offset = -size/2;
-            float p0[3] = {offset, 0, size /4};
-            float p1[3] = {offset, 0, size /4};
+            float offset = -size / 2;
+            float p0[3] = {offset, 0, size / 4};
+            float p1[3] = {offset, 0, size / 4};
             float p2[3] = {offset, -2 * size / 3, 0.0};
             float p3[3] = {offset, 2 * size / 3, 0};
-            float p4[3] = {size*2+offset, 0.0, 0.0};
+            float p4[3] = {size * 2 + offset, 0.0, 0.0};
 
             points->InsertNextPoint(p0);
             points->InsertNextPoint(p1);
@@ -1327,20 +1303,17 @@ void Updater::update_contact_visual(std::shared_ptr<ActorContact> &actor_contact
             points->InsertNextPoint(p3);
             points->InsertNextPoint(p4);
 
-            vtkSmartPointer<vtkPyramid> pyramid =
-                vtkSmartPointer<vtkPyramid>::New();
+            vtkSmartPointer<vtkPyramid> pyramid = vtkSmartPointer<vtkPyramid>::New();
             pyramid->GetPointIds()->SetId(0, 0);
             pyramid->GetPointIds()->SetId(1, 1);
             pyramid->GetPointIds()->SetId(2, 2);
             pyramid->GetPointIds()->SetId(3, 3);
             pyramid->GetPointIds()->SetId(4, 4);
 
-            vtkSmartPointer<vtkCellArray> cells =
-                vtkSmartPointer<vtkCellArray>::New();
+            vtkSmartPointer<vtkCellArray> cells = vtkSmartPointer<vtkCellArray>::New();
             cells->InsertNextCell(pyramid);
 
-            vtkSmartPointer<vtkUnstructuredGrid> ug =
-                vtkSmartPointer<vtkUnstructuredGrid>::New();
+            vtkSmartPointer<vtkUnstructuredGrid> ug = vtkSmartPointer<vtkUnstructuredGrid>::New();
             ug->SetPoints(points);
             ug->InsertNextCell(pyramid->GetCellType(), pyramid->GetPointIds());
 #if VTK_MAJOR_VERSION <= 5
@@ -1350,16 +1323,14 @@ void Updater::update_contact_visual(std::shared_ptr<ActorContact> &actor_contact
 #endif
 
         } else if (actor_contact->contact.type() == sp::SPHERE) {
-            vtkSmartPointer<vtkSphereSource> sphereSource =
-                vtkSmartPointer<vtkSphereSource>::New();
+            vtkSmartPointer<vtkSphereSource> sphereSource = vtkSmartPointer<vtkSphereSource>::New();
             sphereSource->SetCenter(0, 0, 0);
             sphereSource->SetRadius(1);
 
             mapper->SetInputConnection(sphereSource->GetOutputPort());
 
         } else {
-            vtkSmartPointer<vtkSphereSource> sphereSource =
-                vtkSmartPointer<vtkSphereSource>::New();
+            vtkSmartPointer<vtkSphereSource> sphereSource = vtkSmartPointer<vtkSphereSource>::New();
             sphereSource->SetCenter(0, 0, 0);
             sphereSource->SetRadius(1);
 
@@ -1375,36 +1346,30 @@ void Updater::update_contact_visual(std::shared_ptr<ActorContact> &actor_contact
     actor_contact->label->GetProperty()->SetOpacity(cv->opacity());
 
     if (cv->visual_mode() == sp::ContactVisual::COLOR) {
-        actor_contact->actor->GetProperty()->SetColor(cv->color().r()/255.0,
-                                                      cv->color().g()/255.0,
-                                                      cv->color().b()/255.0);
+        actor_contact->actor->GetProperty()->SetColor(
+            cv->color().r() / 255.0, cv->color().g() / 255.0, cv->color().b() / 255.0);
     }
 
     sc::set(actor_contact->color, cv->color());
 }
 
-void Updater::update_trail(std::shared_ptr<ActorContact> &actor_contact,
-                           double &x_pos, double &y_pos, double &z_pos) {
+void Updater::update_trail(std::shared_ptr<ActorContact>& actor_contact, double& x_pos,
+                           double& y_pos, double& z_pos) {
     if (enable_trails_) {
         /////////////////////
         // Create the geometry of a point (the coordinate)
-        vtkSmartPointer<vtkPoints> points =
-            vtkSmartPointer<vtkPoints>::New();
-        const float p[3] =
-            {static_cast<float>(x_pos),
-             static_cast<float>(y_pos),
-             static_cast<float>(z_pos)};
+        vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New();
+        const float p[3] = {static_cast<float>(x_pos), static_cast<float>(y_pos),
+                            static_cast<float>(z_pos)};
 
         // Create the topology of the point (a vertex)
-        vtkSmartPointer<vtkCellArray> vertices =
-            vtkSmartPointer<vtkCellArray>::New();
+        vtkSmartPointer<vtkCellArray> vertices = vtkSmartPointer<vtkCellArray>::New();
         vtkIdType pid[1];
         pid[0] = points->InsertNextPoint(p);
         vertices->InsertNextCell(1, pid);
 
         // Create a polydata object
-        vtkSmartPointer<vtkPolyData> point =
-            vtkSmartPointer<vtkPolyData>::New();
+        vtkSmartPointer<vtkPolyData> point = vtkSmartPointer<vtkPolyData>::New();
 
         // Set the points and vertices we created as the geometry and topology of the polydata
         point->SetPoints(points);
@@ -1420,14 +1385,13 @@ void Updater::update_trail(std::shared_ptr<ActorContact> &actor_contact,
         points_mapper->SetInputData(point);
 #endif
 
-        vtkSmartPointer<vtkActor> points_actor =
-            vtkSmartPointer<vtkActor>::New();
+        vtkSmartPointer<vtkActor> points_actor = vtkSmartPointer<vtkActor>::New();
         points_actor->SetMapper(points_mapper);
         points_actor->GetProperty()->SetPointSize(5);
 
-        points_actor->GetProperty()->SetColor(actor_contact->color.r()/255.0,
-                                              actor_contact->color.g()/255.0,
-                                              actor_contact->color.b()/255.0);
+        points_actor->GetProperty()->SetColor(actor_contact->color.r() / 255.0,
+                                              actor_contact->color.g() / 255.0,
+                                              actor_contact->color.b() / 255.0);
 
         renderer_->AddActor(points_actor);
 
@@ -1447,7 +1411,7 @@ void Updater::update_trail(std::shared_ptr<ActorContact> &actor_contact,
     }
 }
 
-void Updater::process_custom_key(std::string &key) {
+void Updater::process_custom_key(std::string& key) {
     gui_msg_.set_custom_key(key);
     outgoing_interface_->send_gui_msg(gui_msg_);
     gui_msg_.set_custom_key("");
@@ -1495,8 +1459,7 @@ void Updater::request_cached() {
     gui_msg_.set_request_cached(false);
 }
 
-void Updater::world_point_clicked(const double &x, const double &y,
-                                  const double &z) {
+void Updater::world_point_clicked(const double& x, const double& y, const double& z) {
     scrimmage_proto::WorldPointClicked msg;
     sc::set(msg.mutable_point(), x, y, z);
     msg.set_name("WorldPointClicked");
@@ -1505,55 +1468,53 @@ void Updater::world_point_clicked(const double &x, const double &y,
 
 void Updater::toggle_helpmenu() {
     std::stringstream stream_helpkeys, stream_helpvalues;
-        show_helpmenu_ = !show_helpmenu_;
+    show_helpmenu_ = !show_helpmenu_;
     if (show_helpmenu_) {
-        stream_helpkeys
-            << "q\n"
-            << "b\n"
-            << "space\n"
-            << "a\n"
-            << "A\n"
-            << "u\n"
-            << "right/left arrows\n"
-            << "[\n"
-            << "]\n"
-            << "+\n"
-            << "-\n"
-            << "N\n"
-            << "n\n"
-            << "r\n"
-            << "scroll\n"
-            << "z\n"
-            << "SHIFT + z\n"
-            << "w\n"
-            << "s\n"
-            << "CTRL + left click\n"
-            << "SHIFT + left click\n"
-            << ";\n";
+        stream_helpkeys << "q\n"
+                        << "b\n"
+                        << "space\n"
+                        << "a\n"
+                        << "A\n"
+                        << "u\n"
+                        << "right/left arrows\n"
+                        << "[\n"
+                        << "]\n"
+                        << "+\n"
+                        << "-\n"
+                        << "N\n"
+                        << "n\n"
+                        << "r\n"
+                        << "scroll\n"
+                        << "z\n"
+                        << "SHIFT + z\n"
+                        << "w\n"
+                        << "s\n"
+                        << "CTRL + left click\n"
+                        << "SHIFT + left click\n"
+                        << ";\n";
         helpkeys_actor_->SetInput(stream_helpkeys.str().c_str());
-        stream_helpvalues
-            << ": quit\n"
-            << ": pause/unpause\n"
-            << ": step sim (paused only)\n"
-            << ": cycle camera views\n"
-            << ": free camera view\n"
-            << ": undo camera angle change\n"
-            << ": change aircraft\n"
-            << ": decrease warp speed\n"
-            << ": increase warp speed\n"
-            << ": increase visual scale\n"
-            << ": decrease visual scale\n"
-            << ": increase entity label scale\n"
-            << ": decrease entity label scale\n"
-            << ": reset scale/camera\n"
-            << ": zoom\n"
-            << ": zoom out\n"
-            << ": zoom in\n"
-            << ": wireframe\n"
-            << ": solid\n"
-            << ": rotate world\n"
-            << ": pan camera\n"
-            << ": enter custom keypress\n";
+        stream_helpvalues << ": quit\n"
+                          << ": pause/unpause\n"
+                          << ": step sim (paused only)\n"
+                          << ": cycle camera views\n"
+                          << ": free camera view\n"
+                          << ": undo camera angle change\n"
+                          << ": change aircraft\n"
+                          << ": decrease warp speed\n"
+                          << ": increase warp speed\n"
+                          << ": increase visual scale\n"
+                          << ": decrease visual scale\n"
+                          << ": increase entity label scale\n"
+                          << ": decrease entity label scale\n"
+                          << ": reset scale/camera\n"
+                          << ": zoom\n"
+                          << ": zoom out\n"
+                          << ": zoom in\n"
+                          << ": wireframe\n"
+                          << ": solid\n"
+                          << ": rotate world\n"
+                          << ": pan camera\n"
+                          << ": enter custom keypress\n";
         helpvalues_actor_->SetInput(stream_helpvalues.str().c_str());
     } else {
         stream_helpkeys << " ";
@@ -1599,9 +1560,13 @@ void Updater::dec_label_scale() {
     scale_required_ = true;
 }
 
-void Updater::inc_follow_offset() {follow_offset_ *= 1.1;}
+void Updater::inc_follow_offset() {
+    follow_offset_ *= 1.1;
+}
 
-void Updater::dec_follow_offset() {follow_offset_ /= 1.1;}
+void Updater::dec_follow_offset() {
+    follow_offset_ /= 1.1;
+}
 
 void Updater::reset_scale() {
     scale_ = init_scale_;
@@ -1621,8 +1586,8 @@ void Updater::set_show_fps(bool show_fps) {
     show_fps_ = false;
 }
 
-void Updater::set_camera_reset_params(double pos_x, double pos_y, double pos_z,
-                                      double focal_x, double focal_y, double focal_z) {
+void Updater::set_camera_reset_params(double pos_x, double pos_y, double pos_z, double focal_x,
+                                      double focal_y, double focal_z) {
     camera_reset_params_.pos_x = pos_x;
     camera_reset_params_.pos_y = pos_y;
     camera_reset_params_.pos_z = pos_z;
@@ -1716,25 +1681,22 @@ void Updater::create_text_display() {
 
 void Updater::enable_fps() {
     // Set FPS callback:
-    vtkSmartPointer<vtkCallbackCommand> callback =
-        vtkSmartPointer<vtkCallbackCommand>::New();
+    vtkSmartPointer<vtkCallbackCommand> callback = vtkSmartPointer<vtkCallbackCommand>::New();
 
     callback->SetCallback(fpsCallbackFunction);
     renderer_->AddObserver(vtkCommand::EndEvent, callback);
 }
 
-void Updater::quat_2_transform(const sc::Quaternion &q,
-                               vtkSmartPointer<vtkTransform> transform) {
+void Updater::quat_2_transform(const sc::Quaternion& q, vtkSmartPointer<vtkTransform> transform) {
     transform->RotateX(sc::Angles::rad2deg(q.roll()));
     transform->RotateY(sc::Angles::rad2deg(q.pitch()));
     transform->RotateZ(sc::Angles::rad2deg(q.yaw()));
 }
 
-bool Updater::draw_arc(const bool &new_shape,
-                       const scrimmage_proto::Arc &a,
-                       vtkSmartPointer<vtkActor> &actor,
-                       vtkSmartPointer<vtkPolyDataAlgorithm> &source,
-                       vtkSmartPointer<vtkPolyDataMapper> &mapper) {
+bool Updater::draw_arc(const bool& new_shape, const scrimmage_proto::Arc& a,
+                       vtkSmartPointer<vtkActor>& actor,
+                       vtkSmartPointer<vtkPolyDataAlgorithm>& source,
+                       vtkSmartPointer<vtkPolyDataMapper>& mapper) {
     vtkSmartPointer<vtkArcSource> arcSource;
     if (new_shape) {
         arcSource = vtkSmartPointer<vtkArcSource>::New();
@@ -1751,9 +1713,7 @@ bool Updater::draw_arc(const bool &new_shape,
     // get polar vector and normal vector
     Eigen::Vector3d normal = Eigen::Vector3d::UnitZ();
     Eigen::Vector3d polar = Eigen::Vector3d::UnitX() * a.circle().radius();
-    scrimmage::Quaternion quat(a.circle().quat().w(),
-                               a.circle().quat().x(),
-                               a.circle().quat().y(),
+    scrimmage::Quaternion quat(a.circle().quat().w(), a.circle().quat().x(), a.circle().quat().y(),
                                a.circle().quat().z());
     normal = quat.rotate(normal);
     polar = quat.rotate(polar);
@@ -1762,24 +1722,20 @@ bool Updater::draw_arc(const bool &new_shape,
     // NOTE: do not set rotation and normal for the actor if
     // they're being set for the ArcSource itself
     arcSource->SetAngle(sc::Angles::rad2deg(a.angle()));
-    arcSource->SetCenter(a.circle().center().x(),
-                         a.circle().center().y(),
-                         a.circle().center().z());
+    arcSource->SetCenter(a.circle().center().x(), a.circle().center().y(), a.circle().center().z());
     arcSource->SetNormal(normal.data());
     arcSource->SetPolarVector(polar.data());
 
     return true;
 }
 
-bool Updater::draw_arrow(const bool &new_shape,
-                         const scrimmage_proto::Arrow &a,
-                         vtkSmartPointer<vtkActor> &actor,
-                         vtkSmartPointer<vtkPolyDataAlgorithm> &source,
-                         vtkSmartPointer<vtkPolyDataMapper> &mapper) {
+bool Updater::draw_arrow(const bool& new_shape, const scrimmage_proto::Arrow& a,
+                         vtkSmartPointer<vtkActor>& actor,
+                         vtkSmartPointer<vtkPolyDataAlgorithm>& source,
+                         vtkSmartPointer<vtkPolyDataMapper>& mapper) {
     if (new_shape) {
         // Create an arrow.
-        vtkSmartPointer<vtkArrowSource> arrowSource =
-            vtkSmartPointer<vtkArrowSource>::New();
+        vtkSmartPointer<vtkArrowSource> arrowSource = vtkSmartPointer<vtkArrowSource>::New();
 
         double startPoint[3] = {a.tail().x(), a.tail().y(), a.tail().z()};
         double endPoint[3] = {a.head().x(), a.head().y(), a.head().z()};
@@ -1801,8 +1757,7 @@ bool Updater::draw_arrow(const bool &new_shape,
 
         // The Y axis is Z cross X
         vtkMath::Cross(normalizedZ, normalizedX, normalizedY);
-        vtkSmartPointer<vtkMatrix4x4> matrix =
-            vtkSmartPointer<vtkMatrix4x4>::New();
+        vtkSmartPointer<vtkMatrix4x4> matrix = vtkSmartPointer<vtkMatrix4x4>::New();
 
         // Create the direction cosine matrix
         matrix->Identity();
@@ -1813,8 +1768,7 @@ bool Updater::draw_arrow(const bool &new_shape,
         }
 
         // Apply the transforms
-        vtkSmartPointer<vtkTransform> transform =
-            vtkSmartPointer<vtkTransform>::New();
+        vtkSmartPointer<vtkTransform> transform = vtkSmartPointer<vtkTransform>::New();
         transform->Translate(startPoint);
         transform->Concatenate(matrix);
         transform->Scale(length, length, length);
@@ -1837,11 +1791,10 @@ bool Updater::draw_arrow(const bool &new_shape,
     return true;
 }
 
-bool Updater::draw_circle(const bool &new_shape,
-                          const scrimmage_proto::Circle &c,
-                          vtkSmartPointer<vtkActor> &actor,
-                          vtkSmartPointer<vtkPolyDataAlgorithm> &source,
-                          vtkSmartPointer<vtkPolyDataMapper> &mapper) {
+bool Updater::draw_circle(const bool& new_shape, const scrimmage_proto::Circle& c,
+                          vtkSmartPointer<vtkActor>& actor,
+                          vtkSmartPointer<vtkPolyDataAlgorithm>& source,
+                          vtkSmartPointer<vtkPolyDataMapper>& mapper) {
     vtkSmartPointer<vtkRegularPolygonSource> polygonSource;
     if (new_shape) {
         polygonSource = vtkSmartPointer<vtkRegularPolygonSource>::New();
@@ -1862,11 +1815,10 @@ bool Updater::draw_circle(const bool &new_shape,
     return true;
 }
 
-bool Updater::draw_cone(const bool &new_shape,
-                        const scrimmage_proto::Cone &c,
-                        vtkSmartPointer<vtkActor> &actor,
-                        vtkSmartPointer<vtkPolyDataAlgorithm> &source,
-                        vtkSmartPointer<vtkPolyDataMapper> &mapper) {
+bool Updater::draw_cone(const bool& new_shape, const scrimmage_proto::Cone& c,
+                        vtkSmartPointer<vtkActor>& actor,
+                        vtkSmartPointer<vtkPolyDataAlgorithm>& source,
+                        vtkSmartPointer<vtkPolyDataMapper>& mapper) {
     vtkSmartPointer<vtkConeSource> coneSource;
     if (new_shape) {
         coneSource = vtkSmartPointer<vtkConeSource>::New();
@@ -1882,7 +1834,7 @@ bool Updater::draw_cone(const bool &new_shape,
     coneSource->SetResolution(32);
 
     Eigen::Vector3d cone_dir = sc::eigen(c.direction());
-    Eigen::Vector3d apex_shift = sc::eigen(c.apex()) + cone_dir.normalized()*c.height()/2.0;
+    Eigen::Vector3d apex_shift = sc::eigen(c.apex()) + cone_dir.normalized() * c.height() / 2.0;
     Eigen::Vector3d dir_shift = -cone_dir;
     coneSource->SetCenter(apex_shift(0), apex_shift(1), apex_shift(2));
     coneSource->SetDirection(dir_shift(0), dir_shift(1), dir_shift(2));
@@ -1891,11 +1843,10 @@ bool Updater::draw_cone(const bool &new_shape,
     return true;
 }
 
-bool Updater::draw_cube(const bool &new_shape,
-                        const scrimmage_proto::Cuboid &c,
-                        vtkSmartPointer<vtkActor> &actor,
-                        vtkSmartPointer<vtkPolyDataAlgorithm> &source,
-                        vtkSmartPointer<vtkPolyDataMapper> &mapper) {
+bool Updater::draw_cube(const bool& new_shape, const scrimmage_proto::Cuboid& c,
+                        vtkSmartPointer<vtkActor>& actor,
+                        vtkSmartPointer<vtkPolyDataAlgorithm>& source,
+                        vtkSmartPointer<vtkPolyDataMapper>& mapper) {
     vtkSmartPointer<vtkCubeSource> cubeSource;
     if (new_shape) {
         cubeSource = vtkSmartPointer<vtkCubeSource>::New();
@@ -1906,7 +1857,7 @@ bool Updater::draw_cube(const bool &new_shape,
         cubeSource = vtkCubeSource::SafeDownCast(source);
     }
 
-    cubeSource->SetCenter(0, 0, 0); // Place cube at center of actor
+    cubeSource->SetCenter(0, 0, 0);  // Place cube at center of actor
     cubeSource->SetXLength(c.x_length());
     cubeSource->SetYLength(c.y_length());
     cubeSource->SetZLength(c.z_length());
@@ -1917,16 +1868,16 @@ bool Updater::draw_cube(const bool &new_shape,
     return true;
 }
 
-bool Updater::draw_ellipse(const bool &new_shape,
-                           const scrimmage_proto::Ellipse &elp,
-                           vtkSmartPointer<vtkActor> &actor,
-                           vtkSmartPointer<vtkPolyDataAlgorithm> &source,
-                           vtkSmartPointer<vtkPolyDataMapper> &mapper) {
+bool Updater::draw_ellipse(const bool& new_shape, const scrimmage_proto::Ellipse& elp,
+                           vtkSmartPointer<vtkActor>& actor,
+                           vtkSmartPointer<vtkPolyDataAlgorithm>& source,
+                           vtkSmartPointer<vtkPolyDataMapper>& mapper) {
     vtkSmartPointer<vtkRegularPolygonSource> polygonSource;
     if (new_shape) {
         polygonSource = vtkSmartPointer<vtkRegularPolygonSource>::New();
         source = polygonSource;
-        // polygonSource->GeneratePolygonOff(); // Uncomment this line to generate only the outline of the ellipse
+        // polygonSource->GeneratePolygonOff(); // Uncomment this line to generate only the outline
+        // of the ellipse
         polygonSource->SetNumberOfSides(30);
 
         mapper->SetInputConnection(polygonSource->GetOutputPort());
@@ -1942,11 +1893,10 @@ bool Updater::draw_ellipse(const bool &new_shape,
     return true;
 }
 
-bool Updater::draw_line(const bool &new_shape,
-                        const scrimmage_proto::Line &l,
-                        vtkSmartPointer<vtkActor> &actor,
-                        vtkSmartPointer<vtkPolyDataAlgorithm> &source,
-                        vtkSmartPointer<vtkPolyDataMapper> &mapper) {
+bool Updater::draw_line(const bool& new_shape, const scrimmage_proto::Line& l,
+                        vtkSmartPointer<vtkActor>& actor,
+                        vtkSmartPointer<vtkPolyDataAlgorithm>& source,
+                        vtkSmartPointer<vtkPolyDataMapper>& mapper) {
     vtkSmartPointer<vtkLineSource> lineSource;
     if (new_shape) {
         lineSource = vtkSmartPointer<vtkLineSource>::New();
@@ -1958,19 +1908,16 @@ bool Updater::draw_line(const bool &new_shape,
     } else {
         lineSource = vtkLineSource::SafeDownCast(source);
     }
-    lineSource->SetPoint1(l.start().x(), l.start().y(),
-                          l.start().z());
-    lineSource->SetPoint2(l.end().x(), l.end().y(),
-                          l.end().z());
+    lineSource->SetPoint1(l.start().x(), l.start().y(), l.start().z());
+    lineSource->SetPoint2(l.end().x(), l.end().y(), l.end().z());
     lineSource->Update();
     return true;
 }
 
-bool Updater::draw_mesh(const bool &new_shape,
-                        const scrimmage_proto::Mesh &m,
-                        vtkSmartPointer<vtkActor> &actor,
-                        vtkSmartPointer<vtkPolyDataAlgorithm> &source,
-                        vtkSmartPointer<vtkPolyDataMapper> &mapper) {
+bool Updater::draw_mesh(const bool& new_shape, const scrimmage_proto::Mesh& m,
+                        vtkSmartPointer<vtkActor>& actor,
+                        vtkSmartPointer<vtkPolyDataAlgorithm>& source,
+                        vtkSmartPointer<vtkPolyDataMapper>& mapper) {
     vtkSmartPointer<vtkTransformPolyDataFilter> transformFilter;
 
     if (new_shape) {
@@ -1984,14 +1931,11 @@ bool Updater::draw_mesh(const bool &new_shape,
         Quaternion base_rot(0.0, 0.0, 0.0);
 
         // get model, texture, and params for a given model name
-        get_model_texture(m.name(),
-                          model_file, model_file_found,
-                          texture_file, texture_file_found,
+        get_model_texture(m.name(), model_file, model_file_found, texture_file, texture_file_found,
                           base_scale, base_rot);
 
         if (!model_file_found) {
-            std::cout << "Updater: Couldn't find model for "
-                      << m.name() << std::endl;
+            std::cout << "Updater: Couldn't find model for " << m.name() << std::endl;
             return false;
         }
 
@@ -2016,7 +1960,8 @@ bool Updater::draw_mesh(const bool &new_shape,
         transformFilter->SetTransform(transform);
 
         // connect transform filter
-        std::string extension = vtksys::SystemTools::GetFilenameLastExtension(std::string(model_file.c_str()));
+        std::string extension =
+            vtksys::SystemTools::GetFilenameLastExtension(std::string(model_file.c_str()));
         if (extension == ".obj") {
             auto reader = vtkSmartPointer<vtkOBJReader>::New();
             reader->SetFileName(model_file.c_str());
@@ -2045,98 +1990,93 @@ bool Updater::draw_mesh(const bool &new_shape,
     return true;
 }
 
-bool Updater::draw_plane(const bool &new_shape,
-                         const scrimmage_proto::Plane &p,
-                         vtkSmartPointer<vtkActor> &actor,
-                         vtkSmartPointer<vtkPolyDataAlgorithm> &source,
-                         vtkSmartPointer<vtkPolyDataMapper> &mapper) {
-  // sanity checks
-  if (std::abs(p.x_length()) < std::numeric_limits<double>::epsilon()
-      || std::abs(p.y_length()) < std::numeric_limits<double>::epsilon()) {
-    std::cout << "Cannot draw plane: bad dimensions (" << p.x_length() << ", " << p.y_length() << ")\n";
-    return false;
-  }
-  // Load texture
-  std::string texture_file, texture_name;
-  texture_name = p.texture();
-  bool texture_found = false;
-
-  if (!texture_name.empty() && std::string(texture_name) != "") {
-    ConfigParse c_parse;
-    FileSearch file_search;
-    std::map<std::string, std::string> overrides;
-    if (c_parse.parse(overrides, p.texture(), "SCRIMMAGE_DATA_PATH", file_search)) {
-      texture_file = c_parse.directory() + "/" + c_parse.params()["texture"];
-      texture_found = fs::exists(texture_file) && fs::is_regular_file(texture_file);
+bool Updater::draw_plane(const bool& new_shape, const scrimmage_proto::Plane& p,
+                         vtkSmartPointer<vtkActor>& actor,
+                         vtkSmartPointer<vtkPolyDataAlgorithm>& source,
+                         vtkSmartPointer<vtkPolyDataMapper>& mapper) {
+    // sanity checks
+    if (std::abs(p.x_length()) < std::numeric_limits<double>::epsilon()
+        || std::abs(p.y_length()) < std::numeric_limits<double>::epsilon()) {
+        std::cout << "Cannot draw plane: bad dimensions (" << p.x_length() << ", " << p.y_length()
+                  << ")\n";
+        return false;
     }
-  }
-
-  vtkSmartPointer<vtkPlaneSource> planeSource;
-  vtkSmartPointer<vtkJPEGReader> jPEGReader;
-  vtkSmartPointer<vtkTexture> texture;
-  vtkSmartPointer<vtkTextureMapToPlane> texturePlane;
-  jPEGReader = vtkSmartPointer<vtkJPEGReader>::New();
-  texture = vtkSmartPointer<vtkTexture>::New();
-  texturePlane = vtkSmartPointer<vtkTextureMapToPlane>::New();
-  if (new_shape) {
-    planeSource = vtkSmartPointer<vtkPlaneSource>::New();
-    source = planeSource;
-    texture_found ?
-      mapper->SetInputConnection(texturePlane->GetOutputPort()) :
-      mapper->SetInputConnection(planeSource->GetOutputPort());
-    actor->SetMapper(mapper);
-  } else {
-    planeSource = vtkPlaneSource::SafeDownCast(source);
-  }
-
-  // update properties
-  Eigen::Vector3d center, point1, point2;
-  scrimmage::Quaternion quat(
-      p.quat().w(), p.quat().x(), p.quat().y(), p.quat().z());
-  center << p.center().x(), p.center().y(), p.center().z();
-  auto x_hat = quat.rotate(Eigen::Vector3d::UnitX());
-  auto y_hat = quat.rotate(Eigen::Vector3d::UnitY());
-
-  // vtk's "center" is actually the bottom left corner
-  center -= x_hat * (p.x_length() / 2.0) + y_hat * (p.y_length() / 2.0);
-
-  // calculate point1 and point2, which define the plane axes
-  point1 = center + x_hat * p.x_length();
-  point2 = center + y_hat * p.y_length();
-  planeSource->SetOrigin(center[0], center[1], center[2]);
-  planeSource->SetPoint1(point1[0], point1[1], point1[2]);
-  planeSource->SetPoint2(point2[0], point2[1], point2[2]);
-
-  if (texture_found) {
-    actor->SetTexture(texture);
-    jPEGReader->SetFileName(texture_file.c_str());
-    texture->SetInputConnection(jPEGReader->GetOutputPort());
-    texturePlane->SetInputConnection(planeSource->GetOutputPort());
-  }
-
-  // set ambient lighting for the plane
-  if (!p.diffuse_lighting()) {
-    actor->GetProperty()->SetAmbient(1);
-    actor->GetProperty()->SetDiffuse(0);
-  }
-
-  return true;
-}
-
-bool Updater::draw_pointcloud(const bool &new_shape,
-                              const scrimmage_proto::Shape &shape,
-                              vtkSmartPointer<vtkActor> &actor,
-                              vtkSmartPointer<vtkPolyDataAlgorithm> &source,
-                              vtkSmartPointer<vtkPolyDataMapper> &mapper) {
+    // Load texture
+    std::string texture_file, texture_name;
+    texture_name = p.texture();
+    bool texture_found = false;
+
+    if (!texture_name.empty() && std::string(texture_name) != "") {
+        ConfigParse c_parse;
+        FileSearch file_search;
+        std::map<std::string, std::string> overrides;
+        if (c_parse.parse(overrides, p.texture(), "SCRIMMAGE_DATA_PATH", file_search)) {
+            texture_file = c_parse.directory() + "/" + c_parse.params()["texture"];
+            texture_found = fs::exists(texture_file) && fs::is_regular_file(texture_file);
+        }
+    }
+
+    vtkSmartPointer<vtkPlaneSource> planeSource;
+    vtkSmartPointer<vtkJPEGReader> jPEGReader;
+    vtkSmartPointer<vtkTexture> texture;
+    vtkSmartPointer<vtkTextureMapToPlane> texturePlane;
+    jPEGReader = vtkSmartPointer<vtkJPEGReader>::New();
+    texture = vtkSmartPointer<vtkTexture>::New();
+    texturePlane = vtkSmartPointer<vtkTextureMapToPlane>::New();
+    if (new_shape) {
+        planeSource = vtkSmartPointer<vtkPlaneSource>::New();
+        source = planeSource;
+        texture_found ? mapper->SetInputConnection(texturePlane->GetOutputPort())
+                      : mapper->SetInputConnection(planeSource->GetOutputPort());
+        actor->SetMapper(mapper);
+    } else {
+        planeSource = vtkPlaneSource::SafeDownCast(source);
+    }
+
+    // update properties
+    Eigen::Vector3d center, point1, point2;
+    scrimmage::Quaternion quat(p.quat().w(), p.quat().x(), p.quat().y(), p.quat().z());
+    center << p.center().x(), p.center().y(), p.center().z();
+    auto x_hat = quat.rotate(Eigen::Vector3d::UnitX());
+    auto y_hat = quat.rotate(Eigen::Vector3d::UnitY());
+
+    // vtk's "center" is actually the bottom left corner
+    center -= x_hat * (p.x_length() / 2.0) + y_hat * (p.y_length() / 2.0);
+
+    // calculate point1 and point2, which define the plane axes
+    point1 = center + x_hat * p.x_length();
+    point2 = center + y_hat * p.y_length();
+    planeSource->SetOrigin(center[0], center[1], center[2]);
+    planeSource->SetPoint1(point1[0], point1[1], point1[2]);
+    planeSource->SetPoint2(point2[0], point2[1], point2[2]);
+
+    if (texture_found) {
+        actor->SetTexture(texture);
+        jPEGReader->SetFileName(texture_file.c_str());
+        texture->SetInputConnection(jPEGReader->GetOutputPort());
+        texturePlane->SetInputConnection(planeSource->GetOutputPort());
+    }
+
+    // set ambient lighting for the plane
+    if (!p.diffuse_lighting()) {
+        actor->GetProperty()->SetAmbient(1);
+        actor->GetProperty()->SetDiffuse(0);
+    }
+
+    return true;
+}
+
+bool Updater::draw_pointcloud(const bool& new_shape, const scrimmage_proto::Shape& shape,
+                              vtkSmartPointer<vtkActor>& actor,
+                              vtkSmartPointer<vtkPolyDataAlgorithm>& source,
+                              vtkSmartPointer<vtkPolyDataMapper>& mapper) {
     if (new_shape) {
-        const scrimmage_proto::PointCloud &pc = shape.pointcloud();
+        const scrimmage_proto::PointCloud& pc = shape.pointcloud();
 
-        vtkSmartPointer<vtkPoints> points =
-            vtkSmartPointer<vtkPoints>::New();
+        vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New();
 
         for (int i = 0; i < pc.point_size(); i++) {
-            points->InsertNextPoint(pc.point(i).x(), pc.point(i).y(),
-                                    pc.point(i).z());
+            points->InsertNextPoint(pc.point(i).x(), pc.point(i).y(), pc.point(i).z());
         }
 
         vtkSmartPointer<vtkUnsignedCharArray> color_array =
@@ -2170,8 +2110,7 @@ bool Updater::draw_pointcloud(const bool &new_shape,
             }
         }
 
-        vtkSmartPointer<vtkPolyData> pointsPolydata =
-            vtkSmartPointer<vtkPolyData>::New();
+        vtkSmartPointer<vtkPolyData> pointsPolydata = vtkSmartPointer<vtkPolyData>::New();
 
         pointsPolydata->SetPoints(points);
 
@@ -2185,8 +2124,7 @@ bool Updater::draw_pointcloud(const bool &new_shape,
 
         vertexFilter->Update();
 
-        vtkSmartPointer<vtkPolyData> poly_data =
-            vtkSmartPointer<vtkPolyData>::New();
+        vtkSmartPointer<vtkPolyData> poly_data = vtkSmartPointer<vtkPolyData>::New();
         poly_data->ShallowCopy(vertexFilter->GetOutput());
         poly_data->GetPointData()->SetScalars(color_array);
 #if VTK_MAJOR_VERSION < 6
@@ -2201,22 +2139,18 @@ bool Updater::draw_pointcloud(const bool &new_shape,
     return true;
 }
 
-bool Updater::draw_polydata(const bool &new_shape,
-                            const scrimmage_proto::Polydata &p,
-                            vtkSmartPointer<vtkActor> &actor,
-                            vtkSmartPointer<vtkPolyDataAlgorithm> &source,
-                            vtkSmartPointer<vtkPolyDataMapper> &mapper) {
+bool Updater::draw_polydata(const bool& new_shape, const scrimmage_proto::Polydata& p,
+                            vtkSmartPointer<vtkActor>& actor,
+                            vtkSmartPointer<vtkPolyDataAlgorithm>& source,
+                            vtkSmartPointer<vtkPolyDataMapper>& mapper) {
     if (new_shape) {
-        vtkSmartPointer<vtkPoints> points =
-            vtkSmartPointer<vtkPoints>::New();
+        vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New();
 
         for (int i = 0; i < p.point_size(); i++) {
-            points->InsertNextPoint(p.point(i).x(), p.point(i).y(),
-                                    p.point(i).z());
+            points->InsertNextPoint(p.point(i).x(), p.point(i).y(), p.point(i).z());
         }
 
-        vtkSmartPointer<vtkPolyData> polyData =
-            vtkSmartPointer<vtkPolyData>::New();
+        vtkSmartPointer<vtkPolyData> polyData = vtkSmartPointer<vtkPolyData>::New();
         polyData->SetPoints(points);
 
         vtkSmartPointer<vtkVertexGlyphFilter> glyphFilter =
@@ -2237,36 +2171,30 @@ bool Updater::draw_polydata(const bool &new_shape,
     return true;
 }
 
-bool Updater::draw_polygon(const bool &new_shape,
-                           const scrimmage_proto::Polygon &p,
-                           vtkSmartPointer<vtkActor> &actor,
-                           vtkSmartPointer<vtkPolyDataAlgorithm> &source,
-                           vtkSmartPointer<vtkPolyDataMapper> &mapper) {
+bool Updater::draw_polygon(const bool& new_shape, const scrimmage_proto::Polygon& p,
+                           vtkSmartPointer<vtkActor>& actor,
+                           vtkSmartPointer<vtkPolyDataAlgorithm>& source,
+                           vtkSmartPointer<vtkPolyDataMapper>& mapper) {
     if (new_shape) {
         // Setup points
-        vtkSmartPointer<vtkPoints> points =
-            vtkSmartPointer<vtkPoints>::New();
+        vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New();
 
         // Create the polygon
-        vtkSmartPointer<vtkPolygon> polygon =
-            vtkSmartPointer<vtkPolygon>::New();
+        vtkSmartPointer<vtkPolygon> polygon = vtkSmartPointer<vtkPolygon>::New();
 
         polygon->GetPointIds()->SetNumberOfIds(p.point_size());
 
         for (int i = 0; i < p.point_size(); i++) {
-            points->InsertNextPoint(p.point(i).x(), p.point(i).y(),
-                                    p.point(i).z());
+            points->InsertNextPoint(p.point(i).x(), p.point(i).y(), p.point(i).z());
             polygon->GetPointIds()->SetId(i, i);
         }
 
         // Add the polygon to a list of polygons
-        vtkSmartPointer<vtkCellArray> polygons =
-            vtkSmartPointer<vtkCellArray>::New();
+        vtkSmartPointer<vtkCellArray> polygons = vtkSmartPointer<vtkCellArray>::New();
         polygons->InsertNextCell(polygon);
 
         // Create a PolyData
-        vtkSmartPointer<vtkPolyData> polygonPolyData =
-            vtkSmartPointer<vtkPolyData>::New();
+        vtkSmartPointer<vtkPolyData> polygonPolyData = vtkSmartPointer<vtkPolyData>::New();
         polygonPolyData->SetPoints(points);
         polygonPolyData->SetPolys(polygons);
 #if VTK_MAJOR_VERSION < 6
@@ -2281,35 +2209,29 @@ bool Updater::draw_polygon(const bool &new_shape,
     return true;
 }
 
-bool Updater::draw_polyline(const bool &new_shape,
-                            const scrimmage_proto::Polyline &pl,
-                            vtkSmartPointer<vtkActor> &actor,
-                            vtkSmartPointer<vtkPolyDataAlgorithm> &source,
-                            vtkSmartPointer<vtkPolyDataMapper> &mapper) {
+bool Updater::draw_polyline(const bool& new_shape, const scrimmage_proto::Polyline& pl,
+                            vtkSmartPointer<vtkActor>& actor,
+                            vtkSmartPointer<vtkPolyDataAlgorithm>& source,
+                            vtkSmartPointer<vtkPolyDataMapper>& mapper) {
     if (new_shape) {
-        vtkSmartPointer<vtkPoints> points =
-                vtkSmartPointer<vtkPoints>::New();
+        vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New();
 
         for (int i = 0; i < pl.point_size(); i++) {
-            points->InsertNextPoint(pl.point(i).x(), pl.point(i).y(),
-                                    pl.point(i).z());
+            points->InsertNextPoint(pl.point(i).x(), pl.point(i).y(), pl.point(i).z());
         }
 
-        vtkSmartPointer<vtkPolyLine> polyLine =
-                vtkSmartPointer<vtkPolyLine>::New();
+        vtkSmartPointer<vtkPolyLine> polyLine = vtkSmartPointer<vtkPolyLine>::New();
         polyLine->GetPointIds()->SetNumberOfIds(pl.point_size());
         for (int i = 0; i < pl.point_size(); i++) {
             polyLine->GetPointIds()->SetId(i, i);
         }
 
         // Create a cell array to store the lines in and add the lines to it
-        vtkSmartPointer<vtkCellArray> cells =
-                vtkSmartPointer<vtkCellArray>::New();
+        vtkSmartPointer<vtkCellArray> cells = vtkSmartPointer<vtkCellArray>::New();
         cells->InsertNextCell(polyLine);
 
         // Create a polydata to store everything in
-        vtkSmartPointer<vtkPolyData> polyData =
-                vtkSmartPointer<vtkPolyData>::New();
+        vtkSmartPointer<vtkPolyData> polyData = vtkSmartPointer<vtkPolyData>::New();
 
         // Add the points to the dataset
         polyData->SetPoints(points);
@@ -2324,15 +2246,14 @@ bool Updater::draw_polyline(const bool &new_shape,
     return true;
 }
 
-bool Updater::draw_sphere(const bool &new_shape,
-                          const scrimmage_proto::Sphere &s,
-                          vtkSmartPointer<vtkActor> &actor,
-                          vtkSmartPointer<vtkPolyDataAlgorithm> &source,
-                          vtkSmartPointer<vtkPolyDataMapper> &mapper) {
+bool Updater::draw_sphere(const bool& new_shape, const scrimmage_proto::Sphere& s,
+                          vtkSmartPointer<vtkActor>& actor,
+                          vtkSmartPointer<vtkPolyDataAlgorithm>& source,
+                          vtkSmartPointer<vtkPolyDataMapper>& mapper) {
     vtkSmartPointer<vtkSphereSource> sphereSource;
     if (new_shape) {
         sphereSource = vtkSmartPointer<vtkSphereSource>::New();
-        sphereSource->SetCenter(0, 0, 0); // actor is moved later
+        sphereSource->SetCenter(0, 0, 0);  // actor is moved later
         sphereSource->SetThetaResolution(100);
         sphereSource->SetPhiResolution(100);
         source = sphereSource;
@@ -2349,24 +2270,20 @@ bool Updater::draw_sphere(const bool &new_shape,
     return true;
 }
 
-bool Updater::draw_spline(const bool &new_shape,
-                          const scrimmage_proto::Spline &s,
-                          vtkSmartPointer<vtkActor> &actor,
-                          vtkSmartPointer<vtkPolyDataAlgorithm> &source,
-                          vtkSmartPointer<vtkPolyDataMapper> &mapper) {
+bool Updater::draw_spline(const bool& new_shape, const scrimmage_proto::Spline& s,
+                          vtkSmartPointer<vtkActor>& actor,
+                          vtkSmartPointer<vtkPolyDataAlgorithm>& source,
+                          vtkSmartPointer<vtkPolyDataMapper>& mapper) {
     // Setup points
-    vtkSmartPointer<vtkPoints> points =
-        vtkSmartPointer<vtkPoints>::New();
+    vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New();
 
     // FIXME
     for (int i = 0; i < s.point_size(); i++) {
-        points->InsertNextPoint(s.point(i).x(), s.point(i).y(),
-                                s.point(i).z());
+        points->InsertNextPoint(s.point(i).x(), s.point(i).y(), s.point(i).z());
     }
 
     // Create the spline
-    vtkSmartPointer<vtkParametricSpline> spline =
-      vtkSmartPointer<vtkParametricSpline>::New();
+    vtkSmartPointer<vtkParametricSpline> spline = vtkSmartPointer<vtkParametricSpline>::New();
     spline->SetPoints(points);
 
     // set up source for mapper
@@ -2389,11 +2306,10 @@ bool Updater::draw_spline(const bool &new_shape,
     return true;
 }
 
-bool Updater::draw_text(const bool &new_shape,
-                        const scrimmage_proto::Text &t,
-                        vtkSmartPointer<vtkActor> &actor,
-                        vtkSmartPointer<vtkPolyDataAlgorithm> &source,
-                        vtkSmartPointer<vtkPolyDataMapper> &mapper) {
+bool Updater::draw_text(const bool& new_shape, const scrimmage_proto::Text& t,
+                        vtkSmartPointer<vtkActor>& actor,
+                        vtkSmartPointer<vtkPolyDataAlgorithm>& source,
+                        vtkSmartPointer<vtkPolyDataMapper>& mapper) {
     vtkSmartPointer<vtkVectorText> textSource;
     if (new_shape) {
         textSource = vtkSmartPointer<vtkVectorText>::New();
@@ -2418,42 +2334,31 @@ bool Updater::draw_text(const bool &new_shape,
         }
 
         textSource->SetText(t.text().c_str());
-        actor->SetPosition(t.center().x(), t.center().y(),
-                           t.center().z());
+        actor->SetPosition(t.center().x(), t.center().y(), t.center().z());
         return true;
     } else {
         return false;
     }
 }
 
-bool Updater::draw_triangle(const bool &new_shape,
-                            const scrimmage_proto::Triangle &t,
-                            vtkSmartPointer<vtkActor> &actor,
-                            vtkSmartPointer<vtkPolyDataAlgorithm> &source,
-                            vtkSmartPointer<vtkPolyDataMapper> &mapper) {
-    vtkSmartPointer<vtkPoints> points =
-        vtkSmartPointer<vtkPoints>::New();
+bool Updater::draw_triangle(const bool& new_shape, const scrimmage_proto::Triangle& t,
+                            vtkSmartPointer<vtkActor>& actor,
+                            vtkSmartPointer<vtkPolyDataAlgorithm>& source,
+                            vtkSmartPointer<vtkPolyDataMapper>& mapper) {
+    vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New();
 
-    points->InsertNextPoint(t.point0().x(),
-                            t.point0().y(),
-                            t.point0().z());
+    points->InsertNextPoint(t.point0().x(), t.point0().y(), t.point0().z());
 
-    points->InsertNextPoint(t.point1().x(),
-                            t.point1().y(),
-                            t.point1().z());
+    points->InsertNextPoint(t.point1().x(), t.point1().y(), t.point1().z());
 
-    points->InsertNextPoint(t.point2().x(),
-                            t.point2().y(),
-                            t.point2().z());
+    points->InsertNextPoint(t.point2().x(), t.point2().y(), t.point2().z());
 
-    vtkSmartPointer<vtkTriangle> triangle =
-        vtkSmartPointer<vtkTriangle>::New();
+    vtkSmartPointer<vtkTriangle> triangle = vtkSmartPointer<vtkTriangle>::New();
     triangle->GetPointIds()->SetId(0, 0);
     triangle->GetPointIds()->SetId(1, 1);
     triangle->GetPointIds()->SetId(2, 2);
 
-    vtkSmartPointer<vtkCellArray> triangles =
-        vtkSmartPointer<vtkCellArray>::New();
+    vtkSmartPointer<vtkCellArray> triangles = vtkSmartPointer<vtkCellArray>::New();
     triangles->InsertNextCell(triangle);
 
     vtkSmartPointer<vtkPolyData> trianglePolyData;
@@ -2477,10 +2382,9 @@ bool Updater::draw_triangle(const bool &new_shape,
     return true;
 }
 
-void Updater::get_model_texture(std::string name,
-                                std::string& model_file, bool& model_found,
-                                std::string& texture_file, bool& texture_found,
-                                double& base_scale, Quaternion& base_rot) {
+void Updater::get_model_texture(std::string name, std::string& model_file, bool& model_found,
+                                std::string& texture_file, bool& texture_found, double& base_scale,
+                                Quaternion& base_rot) {
     ConfigParse c_parse;
     FileSearch file_search;
     std::map<std::string, std::string> overrides;
@@ -2514,4 +2418,4 @@ void Updater::get_model_texture(std::string name,
         }
     }
 }
-} // namespace scrimmage
+}  // namespace scrimmage
diff --git a/src/viewer/Viewer.cpp b/src/viewer/Viewer.cpp
index 5d400dde58..1abd540edb 100644
--- a/src/viewer/Viewer.cpp
+++ b/src/viewer/Viewer.cpp
@@ -30,26 +30,26 @@
  *
  */
 
-#include <vtkCamera.h>
-
-#include <scrimmage/parse/ParseUtils.h>
-#include <scrimmage/parse/MissionParse.h>
 #include <scrimmage/network/Interface.h>
-#include <scrimmage/viewer/Viewer.h>
-#include <scrimmage/viewer/Updater.h>
+#include <scrimmage/parse/MissionParse.h>
+#include <scrimmage/parse/ParseUtils.h>
 #include <scrimmage/viewer/CameraInterface.h>
+#include <scrimmage/viewer/Updater.h>
+#include <scrimmage/viewer/Viewer.h>
+#include <vtkCamera.h>
 
 #include <boost/algorithm/string.hpp>
 
 namespace scrimmage {
 
-Viewer::Viewer() : enable_network_(false) { }
+Viewer::Viewer() : enable_network_(false) {
+}
 
-void Viewer::set_incoming_interface(InterfacePtr &incoming_interface) {
+void Viewer::set_incoming_interface(InterfacePtr& incoming_interface) {
     incoming_interface_ = incoming_interface;
 }
 
-void Viewer::set_outgoing_interface(InterfacePtr &outgoing_interface) {
+void Viewer::set_outgoing_interface(InterfacePtr& outgoing_interface) {
     outgoing_interface_ = outgoing_interface;
 }
 
@@ -70,8 +70,7 @@ bool Viewer::init(const std::shared_ptr<MissionParse>& mp,
     renderer_->SetBackground(0, 0, 0);
 
     // Setup camera
-    vtkSmartPointer<vtkCamera> camera =
-        vtkSmartPointer<vtkCamera>::New();
+    vtkSmartPointer<vtkCamera> camera = vtkSmartPointer<vtkCamera>::New();
     camera->SetViewUp(0, 0, 1);
     camera->SetPosition(40, 40, 1000);
     camera->SetFocalPoint(0, 0, 0);
@@ -108,7 +107,7 @@ bool Viewer::init(const std::shared_ptr<MissionParse>& mp,
 }
 
 bool Viewer::run() {
-    double update_rate = 50; // Hz
+    double update_rate = 50;  // Hz
 
     if (enable_network_) {
         outgoing_interface_->init_network(Interface::client, remote_ip_, remote_port_);
@@ -127,8 +126,7 @@ bool Viewer::run() {
     renderWindowInteractor_->Initialize();
 
     // Sign up to receive TimerEvent
-    vtkSmartPointer<scrimmage::Updater> updater =
-        vtkSmartPointer<scrimmage::Updater>::New();
+    vtkSmartPointer<scrimmage::Updater> updater = vtkSmartPointer<scrimmage::Updater>::New();
     renderWindowInteractor_->AddObserver(vtkCommand::TimerEvent, updater);
     updater->set_renderer(renderer_);
     updater->set_rwi(renderWindowInteractor_);
@@ -138,8 +136,7 @@ bool Viewer::run() {
     updater->set_init_scale(init_scale_);
     updater->reset_scale();
 
-    std::string camera_pos_str =
-        get<std::string>("pos", camera_params_, "0, 1, 200");
+    std::string camera_pos_str = get<std::string>("pos", camera_params_, "0, 1, 200");
 
     std::vector<double> camera_pos;
     if (!str2container(camera_pos_str, ",", camera_pos, 3)) {
@@ -147,8 +144,7 @@ bool Viewer::run() {
         return false;
     }
 
-    std::string camera_focal_pos_str =
-        get<std::string>("focal_point", camera_params_, "0, 0, 0");
+    std::string camera_focal_pos_str = get<std::string>("focal_point", camera_params_, "0, 0, 0");
 
     std::vector<double> camera_focal_pos;
     if (!str2container(camera_focal_pos_str, ",", camera_focal_pos, 3)) {
@@ -157,7 +153,7 @@ bool Viewer::run() {
     }
 
     updater->set_camera_reset_params(camera_pos[0], camera_pos[1], camera_pos[2],
-        camera_focal_pos[0], camera_focal_pos[1], camera_focal_pos[2]);
+                                     camera_focal_pos[0], camera_focal_pos[1], camera_focal_pos[2]);
     updater->set_show_fps(get("show_fps", camera_params_, false));
 
     updater->set_follow_id(get("follow_id", camera_params_, 1) - 1);
@@ -173,8 +169,8 @@ bool Viewer::run() {
     } else if (view_mode == "OFFSET") {
         updater->set_view_mode(Updater::ViewMode::OFFSET);
     } else {
-        std::cout << "Unrecognized attribute \"" << view_mode
-            << "\" for camera_view_mode" << std::endl;
+        std::cout << "Unrecognized attribute \"" << view_mode << "\" for camera_view_mode"
+                  << std::endl;
         updater->set_view_mode(Updater::ViewMode::FOLLOW);
     }
 
@@ -182,7 +178,7 @@ bool Viewer::run() {
 
     cam_int_->set_updater(updater);
 
-    renderWindowInteractor_->CreateRepeatingTimer(1.0 / update_rate * 1e3); // ms
+    renderWindowInteractor_->CreateRepeatingTimer(1.0 / update_rate * 1e3);  // ms
 
     // Start the interaction and timer
     renderWindowInteractor_->Start();
@@ -192,4 +188,4 @@ bool Viewer::run() {
 
     return true;
 }
-} // namespace scrimmage
+}  // namespace scrimmage