Add max lap speed to lap tracker and enable drivebrain_comms for tests

CMakeLists.txt

@@ -10,7 +10,6 @@ list(APPEND CMAKE_PREFIX_PATH "${CMAKE_SOURCE_DIR}/cmake")
 # Custom libs
 #################################
 add_subdirectory(drivebrain_core)
-# add_subdirectory(drivebrain_comms)
 
 #################################
 # Find packages
@@ -28,7 +27,7 @@ find_package(GTest REQUIRED)
 FetchContent_Declare(
     HT_Proto 
     GIT_REPOSITORY https://github.com/hytech-racing/HT_proto.git
-    GIT_TAG f7967e90831271f9704a406a8f777d43c03d0d0b
+    GIT_TAG 7cdd552
 )
 FetchContent_MakeAvailable(HT_Proto)
 
@@ -45,6 +44,8 @@ FetchContent_Declare(
 )
 FetchContent_MakeAvailable(HT_CAN)
 
+add_subdirectory(drivebrain_comms)
+
 #################################
 # Linking to main app
 #################################

drivebrain_core/include/LapTracker.hpp

@@ -1,47 +1,55 @@
-#include <StateTracker.hpp> 
-#include <hytech_msgs.pb.h> 
+#include <StateTracker.hpp>
+#include <hytech_msgs.pb.h>
+#include <chrono>
 #include <memory>
 
-// TODO Add any imports you might think are neccesary here
-
 namespace core {
 
     class LapTracker {
 
-        public: 
+        public:
 
             /* Singleton move semantics */
             LapTracker(const LapTracker&) = delete;
             LapTracker& operator=(const LapTracker&) = delete;
 
             /**
-             * Steps the lap tracker 
-             * 
-             * @param latest_state the latest state (will be updated in the main loop after get_latest_state_and_validity 
+             * Steps the lap tracker
+             *
+             * @param latest_state the latest state (will be updated in the main loop after get_latest_state_and_validity
              * is called by the state tracker)
              */
             void step_tracker(core::VehicleState& latest_state);
 
             /**
              * Creates a new instance of the lap tracker
              */
-            static void create(); 
-
+            static void create();
             /**
              * Returns the lap tracker instance
              */
-            static LapTracker& instance(); 
-
+            static LapTracker& instance();
+
+            void set_start_finish_line(double start_lat, double start_lon,
+                                      double end_lat, double end_lon);
 
         private:
 
             LapTracker();
-        
+
             /** Internal State */
             inline static std::atomic<LapTracker*> _s_instance;
-
-            // TODO put variables here to keep track of the lap tracker's internal state
-
+
+            std::chrono::steady_clock::time_point _lap_start_time;
+            int _lap_count{0};
+            float _max_lap_speed_mps{0.f};
+
+            double _line_start_lat{0.};
+            double _line_start_lon{0.};
+            double _line_end_lat{0.};
+            double _line_end_lon{0.};
+            double _prev_side{0.};
+            bool _has_prev_position{false};
     };
 
 }

drivebrain_core/include/StateTracker.hpp

@@ -215,6 +215,7 @@ namespace core {
     struct LaptimeInfo {
         float laptime_seconds;
         int lapcount;
+        float max_lap_speed_mps;
     };
 
     /**

drivebrain_core/src/LapTracker.cpp

@@ -1,16 +1,56 @@
-#include <LapTracker.hpp> 
+#include <LapTracker.hpp>
+#include <cmath>
+
+namespace {
+
+double line_side(double line_ax, double line_ay, double line_bx, double line_by,
+                 double px, double py) {
+    double line_dx = line_bx - line_ax;
+    double line_dy = line_by - line_ay;
+    double pt_dx = px - line_ax;
+    double pt_dy = py - line_ay;
+    return pt_dx * line_dy - pt_dy * line_dx;
+}
+
+}  // namespace
 
 void core::LapTracker::step_tracker(core::VehicleState& latest_state) {
-    std::shared_ptr<hytech_msgs::LapTime> laptime_information = std::make_shared<hytech_msgs::LapTime>(); // TODO you need to fill in the fields of this protobuf message
-    /**
-     * TODO it is your responsibility to fill in this method. Look at the VehicleState 
-     * struct to see all the things it gives you (you should see vn position from there). Using this
-     * and the private variables you added in the header, complete this method. It should use all of the information
-     * in the latest state to update it's local variables, create a LapTime protobuf, and invoke handle_receive_protobuf_message
-     * on the state tracker. Some of this is completed for you. Good luck!
-     */
-    core::StateTracker::instance().handle_receive_protobuf_message(laptime_information); // What "records" the information
+    std::shared_ptr<hytech_msgs::LapTime> laptime_information = std::make_shared<hytech_msgs::LapTime>();
 
+    float current_speed_mps = std::sqrt(
+        latest_state.current_body_vel_ms.x * latest_state.current_body_vel_ms.x +
+        latest_state.current_body_vel_ms.y * latest_state.current_body_vel_ms.y +
+        latest_state.current_body_vel_ms.z * latest_state.current_body_vel_ms.z);
+    _max_lap_speed_mps = std::max(_max_lap_speed_mps, current_speed_mps);
+
+    bool line_configured = (_line_start_lat != _line_end_lat) || (_line_start_lon != _line_end_lon);
+    if (latest_state.vehicle_position.valid && line_configured) {
+        double curr_lat = latest_state.vehicle_position.lat;
+        double curr_lon = latest_state.vehicle_position.lon;
+        double curr_side = line_side(_line_start_lat, _line_start_lon,
+                                    _line_end_lat, _line_end_lon,
+                                    curr_lat, curr_lon);
+
+        if (_has_prev_position) {
+            if ((_prev_side > 0 && curr_side < 0) || (_prev_side < 0 && curr_side > 0)) {
+                _lap_count++;
+                _lap_start_time = std::chrono::steady_clock::now();
+                _max_lap_speed_mps = 0.f;
+            }
+        } else {
+            _has_prev_position = true;
+        }
+        _prev_side = curr_side;
+    }
+
+    auto now = std::chrono::steady_clock::now();
+    float laptime_seconds = std::chrono::duration<float>(now - _lap_start_time).count();
+
+    laptime_information->set_laptime_seconds(laptime_seconds);
+    laptime_information->set_lapcount(static_cast<int64_t>(_lap_count));
+    laptime_information->set_max_lap_speed_ms(_max_lap_speed_mps);
+
+    core::StateTracker::instance().handle_receive_protobuf_message(laptime_information);
 }
 
 void core::LapTracker::create() {
@@ -22,6 +62,17 @@ void core::LapTracker::create() {
     }
 }
 
+core::LapTracker::LapTracker()
+    : _lap_start_time(std::chrono::steady_clock::now()) {}
+
+void core::LapTracker::set_start_finish_line(double start_lat, double start_lon,
+                                             double end_lat, double end_lon) {
+    _line_start_lat = start_lat;
+    _line_start_lon = start_lon;
+    _line_end_lat = end_lat;
+    _line_end_lon = end_lon;
+}
+
 core::LapTracker& core::LapTracker::instance() {
     LapTracker* instance = _s_instance.load(std::memory_order_acquire);
     assert(instance != nullptr && "LapTracker has not been initialized");

drivebrain_core/src/StateTracker.cpp

@@ -29,9 +29,15 @@ void StateTracker::handle_receive_protobuf_message(std::shared_ptr<google::proto
 
         auto ins_mode_int = in_msg->status().ins_mode_int();
         auto vel_u = in_msg->status().ins_vel_u();
+        core::Position pos = {
+            static_cast<double>(in_msg->vn_gps().lat()),
+            static_cast<double>(in_msg->vn_gps().lon()),
+            true
+        };
 
         {
             std::unique_lock lk(_state_mutex);
+            _vehicle_state.vehicle_position = pos;
             _vehicle_state.current_body_vel_ms = body_vel_ms;
             _vehicle_state.current_body_accel_mss = body_accel_mss;
             _vehicle_state.current_angular_rate_rads = angular_rate_rads;
@@ -55,9 +61,10 @@ void StateTracker::handle_receive_protobuf_message(std::shared_ptr<google::proto
     } else if (msg->GetDescriptor() == hytech_msgs::LapTime::descriptor()) {
         auto in_msg = std::static_pointer_cast<hytech_msgs::LapTime>(msg);
         {
-            std::unique_lock lk(_state_mutex); 
+            std::unique_lock lk(_state_mutex);
             _vehicle_state.laptime_info.laptime_seconds = in_msg->laptime_seconds();
-            _vehicle_state.laptime_info.lapcount = in_msg->lapcount(); 
+            _vehicle_state.laptime_info.lapcount = static_cast<int>(in_msg->lapcount());
+            _vehicle_state.laptime_info.max_lap_speed_mps = in_msg->max_lap_speed_ms();
         }
     } else {
         _receive_low_level_state(msg);