diff --git a/CMakeLists.txt b/CMakeLists.txt index 01f2742..14b3cea 100644 --- a/CMakeLists.txt +++ b/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}) \ No newline at end of file + DESTINATION ${CMAKE_INSTALL_BINDIR}) diff --git a/drivebrain_core_impl/drivebrain_estimation/include/PerformanceTracker.hpp b/drivebrain_core_impl/drivebrain_estimation/include/PerformanceTracker.hpp index 3a83a2c..73273d2 100644 --- a/drivebrain_core_impl/drivebrain_estimation/include/PerformanceTracker.hpp +++ b/drivebrain_core_impl/drivebrain_estimation/include/PerformanceTracker.hpp @@ -3,6 +3,9 @@ #include #include + +#include +#include // 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> 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 _lap_time; + settings _settings; EventTime _current_event_timing; - - }; -#endif // __PERFORMANCETRACKER_H__ \ No newline at end of file +#endif // __PERFORMANCETRACKER_H__ diff --git a/drivebrain_core_impl/drivebrain_estimation/src/PerformanceTracker.cpp b/drivebrain_core_impl/drivebrain_estimation/src/PerformanceTracker.cpp index e69de29..34c6512 100644 --- a/drivebrain_core_impl/drivebrain_estimation/src/PerformanceTracker.cpp +++ b/drivebrain_core_impl/drivebrain_estimation/src/PerformanceTracker.cpp @@ -0,0 +1,6 @@ +#include + +PerformanceTracker::PerformanceTracker(PerformanceTracker::settings tracker_settings) + : _settings(tracker_settings) {} + +PerformanceTracker::EventTime PerformanceTracker::evaluate_tracker() {} diff --git a/flake.nix b/flake.nix index ff6a58b..cf9d338 100644 --- a/flake.nix +++ b/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 ];