Implement pathing with java side of robot

[nanoticity]

No description provided.

[Copilot]

Pull request overview

This PR adds robot-side pathing and extends the C++ vision stack to support new localization/NT interfaces, along with build/test tooling updates.

Changes:

• Added a new pathing module (grid/BFS + spline/trajectory generation, controller, simulator, and NT publishers).
• Refactored camera + localization pipeline (timestamped frames, new AprilTag detectors/solvers, Pose3d publishing).
• Introduced a shared utils layer (Abseil logging/checks, JSON parsing helpers, transforms, PCH) and reorganized tests/CI scripts.

Reviewed changes

Copilot reviewed 167 out of 246 changed files in this pull request and generated no comments.

Show a summary per file

File	Description
third_party/abseil-cpp	Adds Abseil as a submodule to support new logging/check usage.
third_party/971apriltag/line_fit_filter.cu	Improves CHECK_EQ log message for fixed kernel parameters.
third_party/971apriltag/cuda.h	Switches from local logging macros to Abseil logging/checks.
third_party/971apriltag/CMakeLists.txt	Builds 971apriltag as static and links Abseil log/check.
src/yolo/yolo.h	Updates API signatures (const ref, trailing return types).
src/yolo/yolo.cc	Refactors helper functions and adjusts TensorRT call sites.
src/yolo/model_constants.h	Uses shared PCH include for common headers.
src/yolo/image_collector.cc	Updates camera frame retrieval to new timestamped API.
src/yolo/CMakeLists.txt	Links yolo targets against utils (PCH/logging).
src/utils/transform.h	Adds declarations for OpenCV/Eigen/WPILib transform conversions.
src/utils/transform.cc	Implements transform conversion helpers.
src/utils/timer.h	Adds pragma once + docs; modernizes Stop() signature.
src/utils/timer.cc	Uses move semantics and avoids double-printing on Stop().
src/utils/pch.h	Introduces a precompiled header for common dependencies.
src/utils/nt_utils.h	Adds pragma once and comments for NT setup.
src/utils/nt_utils.cc	Changes NT startup behavior + adds connection wait logging.
src/utils/log.h	Adds utility printing helpers and Abseil logging includes.
src/utils/log.cc	Implements pose/transform/matrix debug printers.
src/utils/constants_from_json.h	Adds helpers to convert intrinsics/extrinsics JSON to types.
src/utils/constants_from_json.cc	Implements JSON conversion specializations.
src/utils/camera_utils.h	Renames/modernizes JSON reading utilities.
src/utils/camera_utils.cc	Routes file open failures through LOG(FATAL).
src/utils/CMakeLists.txt	Expands utils library and enables PCH conditionally.
src/turret_bot_main.cc	Adds a turret-bot main that runs localization threads.
src/test/yolo_test.cc	Updates to new camera frame API + modern main signature.
src/test/unit_test/square_solve_test.cc	Adds gtest coverage for SquareSolver output behavior.
src/test/unit_test/multi_tag_test.cc	Adds gtest to compare multi-tag vs square solver behavior.
src/test/unit_test/matrix.cc	Adds a joint-solver-related matrix test harness.
src/test/unit_test/joint_solve_test.cc	Adds gtest harness for joint solver composition.
src/test/unit_test/CMakeLists.txt	Creates unit test target(s) and enables gtest discovery.
src/test/timer_test.cc	Removes standalone timer test executable.
src/test/tag_estimator_test.cc	Removes old TagEstimator-based test executable.
src/test/stress_test.cc	Removes old stress test executable (moved/replaced).
src/test/path_plan_test.cc	Removes old path plan test (replaced under integration tests).
src/test/intrinsics_test.cc	Adds flags + camera selection; updates frame acquisition.
src/test/integration_test/stress_test.cc	Adds new integration stress test using RunLocalization pipeline.
src/test/integration_test/solver_test.cc	Adds integration solver sanity test harness.
src/test/integration_test/path_plan_test.cc	Adds NT-published path planning visualization + trajectory publish.
src/test/integration_test/networktable_performance_test.cc	Adds an NT queue frequency/period measurement tool.
src/test/integration_test/gamepiece_test_no_img.cc	Adds no-image gamepiece detection integration harness.
src/test/integration_test/gamepiece_test.cc	Adds full gamepiece detection integration harness.
src/test/integration_test/apriltag_detect_test.cc	Adds AprilTag detection integration harness with flags.
src/test/integration_test/CMakeLists.txt	Adds integration test executables and link dependencies.
src/test/camera_source_test.cc	Removes old CameraSource test executable.
src/test/apriltag_detect_test.cc	Removes older apriltag detect test variant.
src/test/CMakeLists.txt	Splits tests into integration_test and unit_test subdirs.
src/software_bot_main.cc	Removes old software bot main (replaced by new mains).
src/pathing/velocity_sender.h	Adds NT publisher for velocity commands to Java side.
src/pathing/velocity_sender.cc	Implements velocity publishing with timestamps.
src/pathing/simulator.cc	Adds offline simulator that logs pose/accel/vel to wpilog.
src/pathing/pathing.h	Declares grid initialization, BFS, and spline/trajectory creation.
src/pathing/pathing.cc	Implements grid init, BFS pathing, and linearized “spline” trajectory.
src/pathing/controller.h	Adds controller reading current/target poses and publishing velocity.
src/pathing/controller.cc	Implements controller logic that generates/follows a trajectory.
src/pathing/CMakeLists.txt	Adds simulator executable and controller library targets.
src/main_bot_main.cc	Adds main robot entrypoint wiring vision + pathing controller toggling.
src/localization/tag_estimator.h	Removes legacy TagEstimator API.
src/localization/tag_estimator.cc	Removes legacy TagEstimator implementation.
src/localization/square_solver.h	Adds SquareSolver interface for position estimation from detections.
src/localization/square_solver.cc	Implements SquareSolver using SOLVEPNP_IPPE_SQUARE + transforms.
src/localization/run_localization.h	Adds a reusable RunLocalization pipeline entrypoint.
src/localization/run_localization.cc	Implements the RunLocalization loop (stream + detect + solve + send).
src/localization/position_solver.h	Adds IPositionSolver interface + shared apriltag constants/layout.
src/localization/position_sender.h	Updates sender to publish Pose2d/Pose3d + accept new estimate type.
src/localization/position_sender.cc	Implements new estimate sender API and Pose3d publishing.
src/localization/position_receiver.h	Adds receiver for Pose3d over NT.
src/localization/position_receiver.cc	Implements Pose3d subscription.
src/localization/position.h	Redefines detection/estimate structs and streaming operator.
src/localization/opencv_apriltag_detector.h	Adds a CPU OpenCV-based AprilTag detector backend.
src/localization/opencv_apriltag_detector.cc	Implements OpenCV AprilTag detection wrapper.
src/localization/nvidia_apriltag_detector.h	Adds a VPI-based AprilTag detector backend.
src/localization/nvidia_apriltag_detector.cc	Implements VPI AprilTag detection wrapper.
src/localization/multi_tag_solver.h	Adds multi-tag PnP solver using multiple tags simultaneously.
src/localization/multi_tag_solver.cc	Implements SQPNP-based multi-tag solve with precomputed tag corners.
src/localization/joint_solver.h	Adds a joint solver API for multi-camera joint solve (WIP).
src/localization/joint_solver.cc	Adds an initial joint solver implementation (currently incomplete).
src/localization/gpu_apriltag_detector.h	Adds GPUAprilTagDetector wrapper for 971apriltag GPU detector.
src/localization/gpu_apriltag_detector.cc	Implements GPU AprilTag detection wrapper producing tag corners.
src/localization/apriltag_detector.h	Adds IAprilTagDetector interface for detector backends.
src/localization/CMakeLists.txt	Replaces TagEstimator build with new detector/solver pipeline sources.
src/gamepiece/gamepiece.h	Adds gamepiece module public API.
src/gamepiece/CMakeLists.txt	Adds gamepiece library target and dependencies.
src/game_piece_main.cc	Converts gamepiece implementation into library functions; removes main.
src/fiddler_main.cc	Updates fiddler main to use RunLocalization pipeline.
src/camera/write_frame.h	Updates WriteFrame signature + uses shared PCH.
src/camera/write_frame.cc	Switches logging output from PNG to JPEG and adjusts compression params.
src/camera/select_camera.h	Adds overloads for camera selection via string/optional.
src/camera/select_camera.cc	Refactors camera selection to use camera_constants names.
src/camera/realsense_camera.h	Updates RealSense camera to timestamped_frame_t GetFrame().
src/camera/realsense_camera.cc	Implements timestamped GetFrame() and logging changes.
src/camera/disk_camera.h	Updates DiskCamera to timestamped_frame_t GetFrame().
src/camera/disk_camera.cc	Implements timestamped GetFrame() and replay timing.
src/camera/cv_camera.h	Updates CVCamera API and adds backup image support.
src/camera/cv_camera.cc	Implements new CVCamera constructors + timestamped GetFrame().
src/camera/cscore_streamer.h	Updates streamer constructor signature and uses PCH.
src/camera/cscore_streamer.cc	Resizes + converts frames before streaming.
src/camera/camera_source.h	Refactors CameraSource storage to use timestamped_frame_t.
src/camera/camera_source.cc	Updates thread loop to fetch timestamped frames from ICamera.
src/camera/camera_constants.h	Expands camera constants, names, and tunables; adds new camera enums.
src/camera/camera.h	Updates ICamera to return timestamped_frame_t.
src/camera/CMakeLists.txt	Links Abseil flags and updates camera lib dependencies.
src/calibration/intrinsics_calibrate_lib.h	Updates calibration API + moves to newer OpenCV aruco interfaces.
src/calibration/intrinsics_calibrate_lib.cc	Implements updated calibration helper API.
src/calibration/intrinsics_calibrate.cc	Adds flags, uses CameraSource, and writes intrinsics to file helper.
src/calibration/frame_shower.cc	Adds flags for camera selection/port; updates frame API.
src/calibration/frame_logger.cc	Adds flags and updates frame API.
src/calibration/focus_calibrate.cc	Updates frame API usage and modernizes main signature.
src/calibration/CMakeLists.txt	Adjusts calibration targets and link dependencies.
src/CMakeLists.txt	Adds clang-tidy option, new subdirs, and new executables.
scripts/run_tests.sh	Adds a script to run unit test binaries from build output.
scripts/deploy.sh	Adds rsync --delete for bin deploy.
scripts/copy_to_bin.sh	Copies artifacts from build/ instead of project root.
scripts/clang_tidy_build.sh	Adds clang-tidy build script for CI.
scripts/build.sh	Expands build script + copies constants to /bos.
docker/wpilib-config.cmake	Removes docker wpilib config file.
docker/lib/nvidia_icd.json	Removes docker NVIDIA ICD config.
docker/lib/libwayland-server.so.0	Removes docker library shim link.
docker/lib/libwayland-egl.so.1	Removes docker library shim link.
docker/lib/libwayland-cursor.so.0	Removes docker library shim link.
docker/lib/libwayland-client.so.0	Removes docker library shim link.
docker/lib/libvulkansc.so.1	Removes docker library shim link.
docker/lib/libv4lconvert.so.0	Removes docker library shim link.
docker/lib/libv4l2.so.0	Removes docker library shim link.
docker/lib/libnvidia-vksc-core.so.1	Removes docker library shim link.
docker/lib/libnvidia-rtcore.so.540.4.0.REMOVED.git-id	Removes docker metadata file.
docker/lib/libnvidia-ptxjitcompiler.so.540.4.0.REMOVED.git-id	Removes docker metadata file.
docker/lib/libnvidia-ptxjitcompiler.so.1	Removes docker library shim link.
docker/lib/libnvidia-nvvm.so.540.4.0.REMOVED.git-id	Removes docker metadata file.
docker/lib/libnvidia-nvvm.so.4	Removes docker library shim link.
docker/lib/libnvidia-gpucomp.so.540.4.0.REMOVED.git-id	Removes docker metadata file.
docker/lib/libnvidia-glvkspirv.so.540.4.0.REMOVED.git-id	Removes docker metadata file.
docker/lib/libnvidia-glcore.so.540.4.0.REMOVED.git-id	Removes docker metadata file.
docker/lib/libnvidia-eglcore.so.540.4.0.REMOVED.git-id	Removes docker metadata file.
docker/lib/libnvidia-egl-wayland.so.1	Removes docker library shim link.
docker/lib/libnvidia-egl-gbm.so.1	Removes docker library shim link.
docker/lib/libnvbufsurftransform.so.1.0.0.REMOVED.git-id	Removes docker metadata file.
docker/lib/libcuda.so.1.1.REMOVED.git-id	Removes docker metadata file.
docker/lib/libcuda.so.1	Removes docker library shim link.
docker/lib/ld.so.conf	Removes docker linker config.
docker/Dockerfile	Removes docker build image definition.
constants/turret_bot/front_right_intrinsics.json	Adds turret bot front-right camera intrinsics.
constants/turret_bot/front_right_extrinsics.json	Adds turret bot front-right camera extrinsics.
constants/turret_bot/front_left_intrinsics.json	Adds turret bot front-left camera intrinsics.
constants/turret_bot/front_left_extrinsics.json	Adds turret bot front-left camera extrinsics.
constants/turret_bot/back_right_intrinsics.json	Adds turret bot back-right camera intrinsics.
constants/turret_bot/back_right_extrinsics.json	Adds turret bot back-right camera extrinsics (placeholder zeros).
constants/realsense_intrinsics.json	Removes RealSense intrinsics constants file.
constants/realsense_extrinsics.json	Removes RealSense extrinsics constants file.
constants/misc/dummy_camera_intrinsics.json	Adds dummy camera intrinsics for tests.
constants/misc/dummy_camera_extrinsics.json	Adds dummy camera extrinsics for tests.
constants/misc/dev_orin_intrinsics.json	Adds dev Orin intrinsics constants file.
constants/misc/dev_orin_extrinsics.json	Adds dev Orin extrinsics constants file.
constants/main_bot/right_intrinsics.json	Adds main robot right camera intrinsics.
constants/main_bot/right_extrinsics.json	Adds main robot right camera extrinsics.
constants/main_bot/left_intrinsics.json	Adds main robot left camera intrinsics.
constants/main_bot/left_extrinsics.json	Adds main robot left camera extrinsics.
constants/main_bot/front_intrinsics.json	Adds main robot front camera intrinsics.
constants/main_bot/front_extrinsics.json	Adds main robot front camera extrinsics.
constants/imx296_camera2_intrinsics.json	Removes unused/zeroed camera2 intrinsics.
constants/imx296_camera2_extrinsics.json	Minor formatting update.
constants/imx296_camera1_intrinsics.json	Removes old IMX296 camera1 intrinsics file.
constants/imx296_camera1_extrinsics.json	Removes old IMX296 camera1 extrinsics file.
constants/camera_constants.json	Adds JSON representation of camera constants.
CMakeLists.txt	Adds Abseil submodule, googletest, VPI dependency, and clang-tidy option.
.gitmodules	Registers abseil-cpp submodule.
.github/workflows/build.yml	Adds self-hosted CI job running clang-tidy build and unit tests.
.clangd	Adds clang-tidy configuration for development.

Comments suppressed due to low confidence (15)

src/camera/cscore_streamer.cc:1

• This unconditionally converts using cv::COLOR_BGRA2BGR, which will throw/assert if frame is already 3-channel (e.g., BGR from a JPEG or some pipelines). Gate the conversion on frame.channels() and use the constructor width/height rather than hardcoding 1080x1080.
  src/camera/disk_camera.cc:1
• After popping image_paths_, the loop compares against image_paths_.top().timestamp (the next frame), and will crash if the queue becomes empty. The wait logic should use the timestamp of the current frame being returned (or delay popping until after the timing check), and must handle the empty-queue case.
  src/pathing/controller.cc:1
• running_ is default-initialized to false and is never set to true in Send(), so the control loop never runs and no velocity commands will be published. Set running_.store(true) at the start of Send() (and consider using exchange(true) to prevent double-start).
  src/localization/nvidia_apriltag_detector.h:1
• This inherits from IAprilTagDetector privately by default, which breaks polymorphic use (e.g., storing as std::unique_ptr<IAprilTagDetector>). Change this to class NvidiaAprilTagDetector : public IAprilTagDetector.
  src/localization/multi_tag_solver.cc:1
• When tag_corners_[detection.tag_id] is missing, the code logs a warning but still calls .value(), which will throw. Skip/continue on invalid tag ids (and also guard against tag ids outside the kmax_tags array bounds).
  src/utils/constants_from_json.cc:1
• The Eigen camera matrix uses fx for the principal point x coordinate (cx) in the (0,2) entry. This should use intrinsics[\"cx\"] rather than repeating fx.
  src/test/unit_test/multi_tag_test.cc:1
• The test intends to compare MultiTagSolver vs SquareSolver, but line 21 calls square_solver again. This makes the test vacuous. Call multi_tag_solver.EstimatePosition(detections)[0] for the multi-tag solution.
  src/localization/position_sender.cc:1
• This declares a 7-element array but only initializes 6 values; the last element will silently default to 0.0. If the NT consumer expects a fixed schema (previously this payload included tag_id), this will break decoding. Either include the missing field(s) (e.g., tag id) or change the array size to match the actual payload.
  src/yolo/yolo.cc:1
• The return values from TensorRT calls are no longer checked (previously asserted). If setTensorAddress or enqueueV3 fails, the code will continue and likely read invalid outputs. Capture and validate the return statuses (LOG(FATAL)/throw or equivalent) to fail fast.
  src/utils/nt_utils.cc:1
• This blocks indefinitely until connected, which can hang integration tests or developer runs when the DS/RIO isn’t reachable. Consider adding a timeout (or a flag to disable waiting) and log a warning/error if the connection isn’t established within a reasonable period.
  src/utils/timer.h:1
• Fix typos in comments to improve readability.
  src/utils/nt_utils.h:1
• Fix typo in comment.
  src/localization/joint_solver.cc:1
• Two correctness issues: (1) w is pre-sized to detections.size() and then emplace_back appends additional entries, leaving the first detections.size() matrices default/empty; indexing w[i] will use the wrong (likely empty) matrices. (2) tag_corners is never initialized before being used. Either build w with reserve() + push_back() (or assign into w[i]), and populate tag_corners from the known tag corner model before projection. As-is, this code is very likely to produce incorrect results or crash.
  src/localization/position_receiver.cc:1
• This subscribes to the empty table with key Pose3d, but PositionSender publishes Pose3d under Orin/PoseEstimate/<camera_name>/Pose3d. As written, PositionReceiver will never receive the published values. Consider taking camera_name (and/or full table path) as a constructor parameter and subscribing to the matching topic.
  src/test/unit_test/CMakeLists.txt:1
• New unit test sources were added (e.g., square_solve_test.cc, joint_solve_test.cc), but only multi_tag_test is built/discovered here. Add executables + gtest_discover_tests() entries for the additional unit tests so they run in CI.