progress on race tracker / performance tracker

CMakeLists.txt

@@ -168,7 +168,6 @@ make_cmake_package(drivebrain_mcap_logger drivebrain)
 add_library(drivebrain_estimation SHARED 
     drivebrain_core_impl/drivebrain_estimation/src/MatlabMath.cpp
     drivebrain_core_impl/drivebrain_estimation/src/StateEstimator.cpp
-    drivebrain_core_impl/drivebrain_estimation/src/RaceTracker.cpp
 )
 
 target_include_directories(drivebrain_estimation PUBLIC
@@ -268,4 +267,4 @@ include(GNUInstallDirs)
 install(TARGETS 
         alpha_build
     RUNTIME 
-        DESTINATION ${CMAKE_INSTALL_BINDIR})
+        DESTINATION ${CMAKE_INSTALL_BINDIR})

drivebrain_core_impl/drivebrain_estimation/include/PerformanceTracker.hpp

@@ -3,6 +3,9 @@
 
 #include <optional>
 #include <vector>
+
+#include <VehicleDataTypes.hpp>
+#include <chrono>
 // goals:
 // - [ ] be able to track lap times
 //      - skid pad
@@ -20,76 +23,97 @@
 /*
 // - [ ] handle geodesic / geographiclib calculations for associated events
 //      - [ ] get geopoints for finish line for accel
-            
-            for the accel event I want to create the accel event bounding 
-            box as well with the left and right side of the track estimated from the resting position of
-            the car itself, assuming it to be in the middle of left and right barriers.
+
+            for the accel event I want to create the accel event bounding
+            box as well with the left and right side of the track estimated from the resting
+position of the car itself, assuming it to be in the middle of left and right barriers.
 
             accel event definition:
 
-            D.9.2.3 "The foremost part of the vehicle will be staged at 0.30 m behind the starting line"
-                - this tells us where in front of the vehicle the start is W.R.T the resting position of the car.
-                    - would be interesting to see how fast we are going when we actually are starting on the start line 
+            D.9.2.3 "The foremost part of the vehicle will be staged at 0.30 m behind the starting
+line"
+                - this tells us where in front of the vehicle the start is W.R.T the resting
+position of the car.
+                    - would be interesting to see how fast we are going when we actually are
+starting on the start line
 
-            D.9.1.2 "Course width will be minimum 4.9 m wide as measured between the inner edges of the bases
-                    of the course edge cones"
+            D.9.1.2 "Course width will be minimum 4.9 m wide as measured between the inner edges of
+the bases of the course edge cones"
                 - 4.9m is the width
 
-            - [ ] get ecef from GEOID forward for the accel-local coordinate system, use the starting rest position 
-                    for the 0,0 in x/y. assume flat ground, z=0 and / or same altitude. 
-                    This position will be the ecef offset for the translation.
-            - [ ] get the online transformation of the current position of the car for accel-centric position 
-                  for visualization and timing purposes.
-            - [ ] timing shall start the moment having crossed the starting line 
-                - our vectornav only has the accuracy ~1m, we will need to see how good it compares to the regular lap timer
-                    - if it is not nearly as good we will need to incorporate the lap timer as a remote input potentially
-        - [ ] handle skidpad event tracking w/ lap time estimates and visualization of estimated track
-                
+            - [ ] get ecef from GEOID forward for the accel-local coordinate system, use the
+starting rest position for the 0,0 in x/y. assume flat ground, z=0 and / or same altitude. This
+position will be the ecef offset for the translation.
+            - [ ] get the online transformation of the current position of the car for accel-centric
+position for visualization and timing purposes.
+            - [ ] timing shall start the moment having crossed the starting line
+                - our vectornav only has the accuracy ~1m, we will need to see how good it compares
+to the regular lap timer
+                    - if it is not nearly as good we will need to incorporate the lap timer as a
+remote input potentially
+        - [ ] handle skidpad event tracking w/ lap time estimates and visualization of estimated
+track
+
 //      - [ ] lap completion logic
 
 // - [ ] create the foxglove schema messages with reference overlays for accel / autox / endurance /
 // skidpad
-
 */
 
+/**
+ * @class PerformanceTracker
+ * @brief
+ *
+ */
 class PerformanceTracker {
   public:
-
+    enum class TrackerMode { NOT_TRACKING, TRACKING_ACCEL, TRACKING_SKIDPAD, TRACKING_AUTOCROSS };
+
     struct GeoPoint {
         double lat;
         double lon;
     };
 
-    struct settings
-    {
+    struct settings {
         float accel_track_length;
         float accel_track_width;
         float accel_starting_line_lead_in_dist;
     };
 
-    struct EventTime
-    {
+    /**
+     * @class EventTime
+     * @brief returned from the evaluation function of the performance tracker. contains the entire
+     * state of the tracker
+     *
+     */
+    struct EventTime {
+        TrackerMode mode;
         /// @brief signifies if an event is currently being timed or not.
-        bool timing_event; 
+        bool timing_event;
         /// @brief the current event time in seconds
         float current_event_time;
 
-        /// @brief optional previous times recorded event times. for endurance this 
-        //         will contain previous lap times and for skid pad it will contain the previous completion time
+        /// @brief optional previous times recorded event times. for endurance this
+        //         will contain previous lap times and for skid pad it will contain the previous
+        //         completion time
         std::optional<std::vector<float>> previous_event_times;
     };
-
 
     PerformanceTracker(settings tracker_settings);
 
-
+    EventTime evaluate_tracker(const core::VehicleState &state);
 
-  private:
+    /**
+     * @brief mode setter for the performance tracker. will be used by the grpc service
+     *
+     * @param mode the mode to change to.
+     */
+    void set_tracker_mode(TrackerMode mode);
 
-
+  private:
+    std::chrono::time_point<std::chrono::high_resolution_clock> _lap_time;
+    settings _settings;
     EventTime _current_event_timing;
-
-
 };
 
-#endif // __PERFORMANCETRACKER_H__
+#endif // __PERFORMANCETRACKER_H__

drivebrain_core_impl/drivebrain_estimation/src/PerformanceTracker.cpp

@@ -0,0 +1,6 @@
+#include <PerformanceTracker.hpp>
+
+PerformanceTracker::PerformanceTracker(PerformanceTracker::settings tracker_settings)
+    : _settings(tracker_settings) {}
+
+PerformanceTracker::EventTime PerformanceTracker::evaluate_tracker() {}

flake.nix

@@ -123,7 +123,7 @@
                   alias run="./build/alpha_build config/drivebrain_config.json $DBC_PATH/hytech.dbc"
                 '';
               nativeBuildInputs = [ pkgs.drivebrain_core_msgs_proto_cpp ];
-              packages = [ pkgs.mcap-cli ];
+              packages = [ pkgs.mcap-cli pkgs.geographiclib ];
               inputsFrom = [
                 pkgs.drivebrain_software
               ];