From 342aaf4858f0e8b51a46bd554f603471ef803a6b Mon Sep 17 00:00:00 2001 From: Viba Mohan Date: Sun, 5 Apr 2026 23:47:00 +0000 Subject: [PATCH 1/4] Auto check localization test --- src/test/integration_test/CMakeLists.txt | 3 + .../auto_check_localization.cc | 83 +++++++++++++++++++ .../integration_test/localization_test2.cc | 2 +- 3 files changed, 87 insertions(+), 1 deletion(-) create mode 100644 src/test/integration_test/auto_check_localization.cc diff --git a/src/test/integration_test/CMakeLists.txt b/src/test/integration_test/CMakeLists.txt index 3206fa4e..d8a43d01 100644 --- a/src/test/integration_test/CMakeLists.txt +++ b/src/test/integration_test/CMakeLists.txt @@ -27,3 +27,6 @@ target_link_libraries(networktable_performance_test PRIVATE utils localization) add_executable(pva_test pva_test.cc) target_link_libraries(pva_test PRIVATE utils localization vpi) + +add_executable(auto_check_localization auto_check_localization.cc) +target_link_libraries(auto_check_localization PRIVATE camera localization utils absl::flags absl::flags_parse wpiutil wpilibc wpimath ntcore cameraserver cscore) diff --git a/src/test/integration_test/auto_check_localization.cc b/src/test/integration_test/auto_check_localization.cc new file mode 100644 index 00000000..c9b09f14 --- /dev/null +++ b/src/test/integration_test/auto_check_localization.cc @@ -0,0 +1,83 @@ +#include +#include +#include +#include +#include +#include +#include + +#include "wpi/DataLogReader.h" +#include "wpi/json.h" +#include "frc/geometry/Pose3d.h" +#include "frc/geometry/Rotation3d.h" +#include "absl/flags/flag.h" +#include "absl/flags/parse.h" + +using NTValue = std::variant, std::vector, + std::vector, frc::Pose3d>; + + +bool decode_record(const wpi::log::DataLogRecord& record, + const wpi::log::StartRecordData& start_data, + NTValue& out_value) { + + if (auto d = record.GetValue()) { + out_value = *d; + } else if (auto f = record.GetValue()) { + out_value = *f; + } else if (auto i = record.GetValue()) { + out_value = *i; + } else if (auto b = record.GetValue()) { + out_value = *b; + } else if (auto s = record.GetValue()) { + out_value = *s; + } else if (auto arr_d = record.GetValue>()) { + out_value = *arr_d; + } else if (auto arr_f = record.GetValue>()) { + out_value = *arr_f; + } else if (auto arr_i = record.GetValue>()) { + out_value = *arr_i; + } else { + // fallback: treat raw as Pose3d if needed + if (auto raw = record.GetValue()) { + out_value = *raw; + } else { + return false; + } + } + return true; +} + +int main(int argc, char** argv) { + absl::ParseCommandLine(argc, argv); + + std::string log_path = "path_to_log.dlog"; + wpi::log::DataLogReader reader(log_path); + + std::map>> nt_timeseries; + + for (const auto& record : reader) { + if (record.type != wpi::log::RecordType::kData) continue; + + NTValue value; + if (!decode_record(record, reader.GetStart(record.name), value)) continue; + + nt_timeseries[std::string(record.name)].emplace_back(record.timestamp, value); + } + + double delta_rot_threshold = 0.05; + for (auto& [name, series] : nt_timeseries) { + for (auto& [timestamp, val] : series) { + if (std::holds_alternative(val)) { + auto pose = std::get(val); + auto rpy = pose.Rotation().ToYawPitchRoll(); + if (std::abs(rpy[0]) > delta_rot_threshold) { + std::cout << "Yaw exceeded threshold at " << timestamp << "\n"; + } + } + } + } + + return 0; +} \ No newline at end of file diff --git a/src/test/integration_test/localization_test2.cc b/src/test/integration_test/localization_test2.cc index 421be66f..290406a6 100644 --- a/src/test/integration_test/localization_test2.cc +++ b/src/test/integration_test/localization_test2.cc @@ -52,4 +52,4 @@ auto main(int argc, char** argv) -> int { localization::UnambiguousEstimator estimator( cameras, std::make_optional(5801), false, paths); estimator.Run(); -} +} \ No newline at end of file From a81bbb967e2aa4db6eaaa8910a612bc448927f9a Mon Sep 17 00:00:00 2001 From: Viba Mohan Date: Mon, 6 Apr 2026 00:35:12 +0000 Subject: [PATCH 2/4] it builds now --- .../auto_check_localization.cc | 138 ++++++++++-------- 1 file changed, 77 insertions(+), 61 deletions(-) diff --git a/src/test/integration_test/auto_check_localization.cc b/src/test/integration_test/auto_check_localization.cc index c9b09f14..cfad6404 100644 --- a/src/test/integration_test/auto_check_localization.cc +++ b/src/test/integration_test/auto_check_localization.cc @@ -1,83 +1,99 @@ #include -#include #include #include -#include #include -#include #include "wpi/DataLogReader.h" -#include "wpi/json.h" -#include "frc/geometry/Pose3d.h" -#include "frc/geometry/Rotation3d.h" +#include "wpi/MemoryBuffer.h" #include "absl/flags/flag.h" #include "absl/flags/parse.h" -using NTValue = std::variant, std::vector, - std::vector, frc::Pose3d>; - - -bool decode_record(const wpi::log::DataLogRecord& record, - const wpi::log::StartRecordData& start_data, - NTValue& out_value) { - - if (auto d = record.GetValue()) { - out_value = *d; - } else if (auto f = record.GetValue()) { - out_value = *f; - } else if (auto i = record.GetValue()) { - out_value = *i; - } else if (auto b = record.GetValue()) { - out_value = *b; - } else if (auto s = record.GetValue()) { - out_value = *s; - } else if (auto arr_d = record.GetValue>()) { - out_value = *arr_d; - } else if (auto arr_f = record.GetValue>()) { - out_value = *arr_f; - } else if (auto arr_i = record.GetValue>()) { - out_value = *arr_i; - } else { - // fallback: treat raw as Pose3d if needed - if (auto raw = record.GetValue()) { - out_value = *raw; - } else { - return false; - } - } - return true; -} +ABSL_FLAG(std::string, input_file, "", "Path to the wpilog file"); +ABSL_FLAG(std::string, entry_name, "NT:/Orin/PoseEstimate/Left/Pose3d", + "Name of the entry to extract"); int main(int argc, char** argv) { - absl::ParseCommandLine(argc, argv); + absl::ParseCommandLine(argc, argv); - std::string log_path = "path_to_log.dlog"; - wpi::log::DataLogReader reader(log_path); + const std::string file_path = absl::GetFlag(FLAGS_input_file); + const std::string target_entry = absl::GetFlag(FLAGS_entry_name); - std::map>> nt_timeseries; + if (file_path.empty()) { + std::cerr << "Error: --input_file flag is required\n"; + return 1; + } - for (const auto& record : reader) { - if (record.type != wpi::log::RecordType::kData) continue; + // Load the wpilog file into a MemoryBuffer + std::error_code ec; + auto buffer = wpi::MemoryBuffer::GetFileAsStream(file_path, ec); + if (ec) { + std::cerr << "Error: Could not open file " << file_path << ": " + << ec.message() << "\n"; + return 1; + } - NTValue value; - if (!decode_record(record, reader.GetStart(record.name), value)) continue; + // Create the DataLogReader + wpi::log::DataLogReader reader(std::move(buffer)); + if (!reader.IsValid()) { + std::cerr << "Error: Data log is invalid\n"; + return 1; + } - nt_timeseries[std::string(record.name)].emplace_back(record.timestamp, value); - } + std::cout << "Data log version: 0x" << std::hex << reader.GetVersion() + << std::dec << "\n"; + std::cout << "Processing log: " << file_path << "\n\n"; - double delta_rot_threshold = 0.05; - for (auto& [name, series] : nt_timeseries) { - for (auto& [timestamp, val] : series) { - if (std::holds_alternative(val)) { - auto pose = std::get(val); - auto rpy = pose.Rotation().ToYawPitchRoll(); - if (std::abs(rpy[0]) > delta_rot_threshold) { - std::cout << "Yaw exceeded threshold at " << timestamp << "\n"; - } - } + // Map entry IDs to their start record data + std::map entry_map; + int target_entry_id = -1; + + // First pass: build entry map and find target entry + for (const auto& record : reader) { + if (record.IsStart()) { + wpi::log::StartRecordData start_data; + if (record.GetStartData(&start_data)) { + entry_map[start_data.entry] = start_data; + + if (std::string(start_data.name) == target_entry) { + target_entry_id = start_data.entry; + std::cout << "Found target entry: " << start_data.name << "\n"; + std::cout << " Entry ID: " << target_entry_id << "\n"; + std::cout << " Type: " << start_data.type << "\n\n"; } + } } + } + if (target_entry_id == -1) { + std::cout << "Target entry '" << target_entry << "' not found in log\n"; return 0; + } + + // Second pass: extract Pose3d data for target entry + std::cout << "Pose3d records for '" << target_entry << "':\n"; + int record_count = 0; + + // Reload the reader for the second pass + buffer = wpi::MemoryBuffer::GetFileAsStream(file_path, ec); + if (ec) { + std::cerr << "Error: Could not reopen file\n"; + return 1; + } + wpi::log::DataLogReader reader2(std::move(buffer)); + + for (const auto& record : reader2) { + if (!record.IsControl() && record.GetEntry() == target_entry_id) { + record_count++; + const int64_t timestamp = record.GetTimestamp(); + auto raw_data = record.GetRaw(); + + std::cout << "[" << record_count << "] Timestamp: " << timestamp + << " us, Size: " << raw_data.size() << " bytes\n"; + } + } + + std::cout << "\n"; + std::cout << "Total Pose3d records: " << record_count << "\n"; + + return 0; } \ No newline at end of file From 4d240dc25089c0797dd8c879899759b19d3e5450 Mon Sep 17 00:00:00 2001 From: Viba Mohan Date: Mon, 6 Apr 2026 01:18:06 +0000 Subject: [PATCH 3/4] use normal rotations instead --- .../auto_check_localization.cc | 103 +++++++++++++++--- 1 file changed, 90 insertions(+), 13 deletions(-) diff --git a/src/test/integration_test/auto_check_localization.cc b/src/test/integration_test/auto_check_localization.cc index cfad6404..442f00b5 100644 --- a/src/test/integration_test/auto_check_localization.cc +++ b/src/test/integration_test/auto_check_localization.cc @@ -2,11 +2,17 @@ #include #include #include +#include +#include #include "wpi/DataLogReader.h" #include "wpi/MemoryBuffer.h" +#include "frc/geometry/Pose3d.h" +#include "frc/geometry/Translation3d.h" +#include "frc/geometry/Rotation3d.h" #include "absl/flags/flag.h" #include "absl/flags/parse.h" +#include "src/utils/log.h" ABSL_FLAG(std::string, input_file, "", "Path to the wpilog file"); ABSL_FLAG(std::string, entry_name, "NT:/Orin/PoseEstimate/Left/Pose3d", @@ -23,7 +29,6 @@ int main(int argc, char** argv) { return 1; } - // Load the wpilog file into a MemoryBuffer std::error_code ec; auto buffer = wpi::MemoryBuffer::GetFileAsStream(file_path, ec); if (ec) { @@ -32,7 +37,6 @@ int main(int argc, char** argv) { return 1; } - // Create the DataLogReader wpi::log::DataLogReader reader(std::move(buffer)); if (!reader.IsValid()) { std::cerr << "Error: Data log is invalid\n"; @@ -43,11 +47,9 @@ int main(int argc, char** argv) { << std::dec << "\n"; std::cout << "Processing log: " << file_path << "\n\n"; - // Map entry IDs to their start record data std::map entry_map; int target_entry_id = -1; - // First pass: build entry map and find target entry for (const auto& record : reader) { if (record.IsStart()) { wpi::log::StartRecordData start_data; @@ -69,11 +71,10 @@ int main(int argc, char** argv) { return 0; } - // Second pass: extract Pose3d data for target entry std::cout << "Pose3d records for '" << target_entry << "':\n"; - int record_count = 0; + + std::vector>> pose_records; - // Reload the reader for the second pass buffer = wpi::MemoryBuffer::GetFileAsStream(file_path, ec); if (ec) { std::cerr << "Error: Could not reopen file\n"; @@ -83,17 +84,93 @@ int main(int argc, char** argv) { for (const auto& record : reader2) { if (!record.IsControl() && record.GetEntry() == target_entry_id) { - record_count++; const int64_t timestamp = record.GetTimestamp(); auto raw_data = record.GetRaw(); - - std::cout << "[" << record_count << "] Timestamp: " << timestamp - << " us, Size: " << raw_data.size() << " bytes\n"; + pose_records.emplace_back(timestamp, + std::vector(raw_data.begin(), raw_data.end())); } } - std::cout << "\n"; - std::cout << "Total Pose3d records: " << record_count << "\n"; + std::cout << "Total Pose3d records: " << pose_records.size() << "\n\n"; + + const double rotation_threshold = 0.1; + const double translation_threshold = 0.5; + + std::cout << "Analyzing consecutive Pose3d changes:\n"; + for (size_t i = 1; i < pose_records.size(); i++) { + const auto& prev_record = pose_records[i - 1]; + const auto& curr_record = pose_records[i]; + + int64_t curr_timestamp = curr_record.first; + const auto& prev_data = prev_record.second; + const auto& curr_data = curr_record.second; + + try { + if (prev_data.size() < 56 || curr_data.size() < 56) { + LOG(ERROR) << "Invalid Pose3d data size"; + continue; + } + + auto read_double = [](const uint8_t* data, size_t offset) -> double { + double value; + std::memcpy(&value, data + offset, sizeof(double)); + return value; + }; + + double prev_px = read_double(prev_data.data(), 0); + double prev_py = read_double(prev_data.data(), 8); + double prev_pz = read_double(prev_data.data(), 16); + double prev_qw = read_double(prev_data.data(), 24); + double prev_qx = read_double(prev_data.data(), 32); + double prev_qy = read_double(prev_data.data(), 40); + double prev_qz = read_double(prev_data.data(), 48); + + frc::Pose3d prev_pose( + frc::Translation3d(units::meter_t(prev_px), units::meter_t(prev_py), + units::meter_t(prev_pz)), + frc::Rotation3d(frc::Quaternion(prev_qw, prev_qx, prev_qy, prev_qz))); + + double curr_px = read_double(curr_data.data(), 0); + double curr_py = read_double(curr_data.data(), 8); + double curr_pz = read_double(curr_data.data(), 16); + double curr_qw = read_double(curr_data.data(), 24); + double curr_qx = read_double(curr_data.data(), 32); + double curr_qy = read_double(curr_data.data(), 40); + double curr_qz = read_double(curr_data.data(), 48); + + frc::Pose3d curr_pose( + frc::Translation3d(units::meter_t(curr_px), units::meter_t(curr_py), + units::meter_t(curr_pz)), + frc::Rotation3d(frc::Quaternion(curr_qw, curr_qx, curr_qy, curr_qz))); + + const auto transl_delta = + curr_pose.Translation().Distance(prev_pose.Translation()); + + auto prev_rot = prev_pose.Rotation(); + auto curr_rot = curr_pose.Rotation(); + double prev_yaw = prev_rot.Z().value(); + double curr_yaw = curr_rot.Z().value(); + double yaw_delta = std::abs(curr_yaw - prev_yaw); + while (yaw_delta > M_PI) yaw_delta -= 2 * M_PI; + double rotation_delta = std::abs(yaw_delta); + + if (transl_delta.value() > translation_threshold) { + LOG(WARNING) << "Translation delta exceeded threshold at timestamp " + << curr_timestamp << " us: " << transl_delta.value() + << " m (threshold: " << translation_threshold << " m)"; + } + + if (rotation_delta > rotation_threshold) { + LOG(WARNING) << "Rotation delta exceeded threshold at timestamp " + << curr_timestamp << " us: " << rotation_delta + << " rad (threshold: " << rotation_threshold << " rad)"; + } + + } catch (const std::exception& e) { + LOG(ERROR) << "Failed to deserialize Pose3d at timestamp " + << curr_timestamp << ": " << e.what(); + } + } return 0; } \ No newline at end of file From 5ae5182c7772c1f8f59d38127fbd7741e9ac500d Mon Sep 17 00:00:00 2001 From: Viba Mohan Date: Wed, 8 Apr 2026 10:21:03 -0700 Subject: [PATCH 4/4] Remove redundant deps from the target link Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> Signed-off-by: Viba Mohan --- src/test/integration_test/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/test/integration_test/CMakeLists.txt b/src/test/integration_test/CMakeLists.txt index d8a43d01..cc2d7a62 100644 --- a/src/test/integration_test/CMakeLists.txt +++ b/src/test/integration_test/CMakeLists.txt @@ -29,4 +29,4 @@ add_executable(pva_test pva_test.cc) target_link_libraries(pva_test PRIVATE utils localization vpi) add_executable(auto_check_localization auto_check_localization.cc) -target_link_libraries(auto_check_localization PRIVATE camera localization utils absl::flags absl::flags_parse wpiutil wpilibc wpimath ntcore cameraserver cscore) +target_link_libraries(auto_check_localization PRIVATE camera localization utils)