From fc5808d3be5db0aa39fd61e4790214aae535083d Mon Sep 17 00:00:00 2001 From: nm121212 <127545866+nm121212@users.noreply.github.com> Date: Mon, 23 Feb 2026 16:13:50 -0500 Subject: [PATCH] Add max lap speed to lap tracker and enable drivebrain_comms for tests Co-authored-by: Cursor --- CMakeLists.txt | 5 +- drivebrain_core/include/LapTracker.hpp | 40 +++++++------ drivebrain_core/include/StateTracker.hpp | 1 + drivebrain_core/src/LapTracker.cpp | 71 ++++++++++++++++++++---- drivebrain_core/src/StateTracker.cpp | 11 +++- 5 files changed, 98 insertions(+), 30 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 6b8dc36..0fb9095 100644 --- a/CMakeLists.txt +++ b/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 ################################# diff --git a/drivebrain_core/include/LapTracker.hpp b/drivebrain_core/include/LapTracker.hpp index 319f8b6..707ab97 100644 --- a/drivebrain_core/include/LapTracker.hpp +++ b/drivebrain_core/include/LapTracker.hpp @@ -1,23 +1,22 @@ -#include -#include +#include +#include +#include #include -// 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); @@ -25,23 +24,32 @@ namespace core { /** * 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 _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}; }; } \ No newline at end of file diff --git a/drivebrain_core/include/StateTracker.hpp b/drivebrain_core/include/StateTracker.hpp index bb24a71..7c5a1b5 100644 --- a/drivebrain_core/include/StateTracker.hpp +++ b/drivebrain_core/include/StateTracker.hpp @@ -215,6 +215,7 @@ namespace core { struct LaptimeInfo { float laptime_seconds; int lapcount; + float max_lap_speed_mps; }; /** diff --git a/drivebrain_core/src/LapTracker.cpp b/drivebrain_core/src/LapTracker.cpp index 70c9b36..611c8d5 100644 --- a/drivebrain_core/src/LapTracker.cpp +++ b/drivebrain_core/src/LapTracker.cpp @@ -1,16 +1,56 @@ -#include +#include +#include + +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 laptime_information = std::make_shared(); // 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 laptime_information = std::make_shared(); + 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(now - _lap_start_time).count(); + + laptime_information->set_laptime_seconds(laptime_seconds); + laptime_information->set_lapcount(static_cast(_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"); diff --git a/drivebrain_core/src/StateTracker.cpp b/drivebrain_core/src/StateTracker.cpp index e414667..8ec7c40 100644 --- a/drivebrain_core/src/StateTracker.cpp +++ b/drivebrain_core/src/StateTracker.cpp @@ -29,9 +29,15 @@ void StateTracker::handle_receive_protobuf_message(std::shared_ptrstatus().ins_mode_int(); auto vel_u = in_msg->status().ins_vel_u(); + core::Position pos = { + static_cast(in_msg->vn_gps().lat()), + static_cast(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_ptrGetDescriptor() == hytech_msgs::LapTime::descriptor()) { auto in_msg = std::static_pointer_cast(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(in_msg->lapcount()); + _vehicle_state.laptime_info.max_lap_speed_mps = in_msg->max_lap_speed_ms(); } } else { _receive_low_level_state(msg);