diff --git a/.gitignore b/.gitignore index 8f7a1951..5bdc77b6 100644 --- a/.gitignore +++ b/.gitignore @@ -20,6 +20,10 @@ models/* images/* !images/.gitkeep +!images/mapping/ + +images/mapping/* +!images/mapping/.gitkeep tests/integration/images/* !tests/integration/images/.gitkeep @@ -38,9 +42,18 @@ tests/integration/mapping/batch2/* tests/integration/mapping/images/* !tests/integration/mapping/images/.gitkeep +tests/integration/mapping/direct_test_images/* +!tests/integration/mapping/direct_test_images/.gitkeep + tests/integration/mapping/output/* !tests/integration/mapping/output/.gitkeep +tests/integration/mapping/direct_output/* +!tests/integration/mapping/direct_output/.gitkeep + +tests/integration/aggregator/* +!tests/integration/aggregator/.gitkeep + .vscode/* !.vscode/c_cpp_properties.json diff --git a/CMakeLists.txt b/CMakeLists.txt index 8456a25b..72909f55 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -61,6 +61,7 @@ set(LOGURU_WITH_STREAMS TRUE) set(FETCHCONTENT_QUIET FALSE) set(DEPS_DIRECTORY ${PROJECT_SOURCE_DIR}/deps) +include(${DEPS_DIRECTORY}/onnxruntime/onnxruntime.cmake) include(${DEPS_DIRECTORY}/torch/torch.cmake) include(${DEPS_DIRECTORY}/torchvision/torchvision.cmake) include(${DEPS_DIRECTORY}/json/json.cmake) @@ -193,3 +194,16 @@ else() message(FATAL_ERROR "cpplint executable not found. Check the README for steps to install it on your system") endif() # ============================= + +add_custom_target(dev_protos + COMMAND cd ${CMAKE_BINARY_DIR} && + cmake .. && + cd ${CMAKE_SOURCE_DIR}/include/udp_squared && + git checkout feat/udp_update && + cd ${CMAKE_SOURCE_DIR}/protos && + git checkout feat/protoupdate && + cd ${CMAKE_BINARY_DIR} && + protoc -I=../protos --cpp_out=./gen_protos/protos ../protos/obc.proto && + ninja obcpp + USES_TERMINAL +) diff --git a/configs/dev.json b/configs/dev.json index eef96058..ebf864b7 100644 --- a/configs/dev.json +++ b/configs/dev.json @@ -13,7 +13,8 @@ } }, "takeoff": { - "altitude_m": 30.0 + "altitude_m": 30.0, + "payload_size": 2 }, "pathing": { "dubins": { @@ -25,11 +26,12 @@ "rewire_radius": 256.0, "optimize": true, "point_fetch_method": "nearest", - "allowed_to_skip_waypoints": false + "allowed_to_skip_waypoints": false, + "generate_deviations": false }, "coverage": { "altitude_m": 30.0, - "camera_vision_m": 5.0, + "camera_vision_m": 20.0, "method": "hover", "hover": { "pictures_per_stop": 1, @@ -50,9 +52,7 @@ } }, "cv": { - "matching_model_dir": "/workspaces/obcpp/models/target_siamese_1.pt", - "segmentation_model_dir": "/workspaces/obcpp/models/fcn-model_20-epochs_06-01-2023T21-16-02.pth", - "saliency_model_dir": "/workspaces/obcpp/models/torchscript_19.pth", + "yolo_model_dir": "/workspaces/obcpp/models/yolo11x.onnx", "not_stolen_addr": "localhost", "not_stolen_port": 5069 }, diff --git a/configs/jetson.json b/configs/jetson.json index 80c076fb..72244d59 100644 --- a/configs/jetson.json +++ b/configs/jetson.json @@ -50,9 +50,7 @@ } }, "cv": { - "matching_model_dir": "/obcpp/models/target_siamese_1.pt", - "segmentation_model_dir": "/obcpp/models/fcn-model_20-epochs_06-01-2023T21-16-02.pth", - "saliency_model_dir": "/obcpp/models/torchscript_19.pth", + "yolo_model_dir": "/obcpp/models/yolo11x.onnx", "not_stolen_addr": "localhost", "not_stolen_port": 5069 }, diff --git a/deps/onnxruntime/onnxruntime.cmake b/deps/onnxruntime/onnxruntime.cmake new file mode 100644 index 00000000..cfb6279d --- /dev/null +++ b/deps/onnxruntime/onnxruntime.cmake @@ -0,0 +1,40 @@ +function(target_add_onnxruntime target_name) + include(FetchContent) + + # Detect architecture. + if(CMAKE_SYSTEM_PROCESSOR STREQUAL "x86_64") + set(ONNX_URL "https://github.com/microsoft/onnxruntime/releases/download/v1.20.1/onnxruntime-linux-x64-1.20.1.tgz") + set(ONNX_HASH SHA256=67db4dc1561f1e3fd42e619575c82c601ef89849afc7ea85a003abbac1a1a105) + elseif(CMAKE_SYSTEM_PROCESSOR STREQUAL "aarch64") + set(ONNX_URL "https://github.com/microsoft/onnxruntime/releases/download/v1.20.1/onnxruntime-linux-aarch64-1.20.1.tgz") + set(ONNX_HASH SHA256=ae4fedbdc8c18d688c01306b4b50c63de3445cdf2dbd720e01a2fa3810b8106a) + else() + message(FATAL_ERROR "Unknown architecture: ${CMAKE_SYSTEM_PROCESSOR}") + endif() + + FetchContent_Declare( + onnxruntime + URL ${ONNX_URL} + URL_HASH ${ONNX_HASH} + DOWNLOAD_EXTRACT_TIMESTAMP FALSE + ) + FetchContent_MakeAvailable(onnxruntime) + + # Apply patches + if(EXISTS "${onnxruntime_SOURCE_DIR}/include/onnxruntime_cxx_api.h") + execute_process( + COMMAND sed -i "210s|^|//|" "${onnxruntime_SOURCE_DIR}/include/onnxruntime_cxx_api.h" + ) + execute_process( + COMMAND sed -i "351s|^|//|" "${onnxruntime_SOURCE_DIR}/include/onnxruntime_cxx_api.h" + ) + endif() + + target_include_directories(${target_name} PUBLIC + ${onnxruntime_SOURCE_DIR}/include + ) + target_link_directories(${target_name} PUBLIC + ${onnxruntime_SOURCE_DIR}/lib + ) + target_link_libraries(${target_name} PUBLIC onnxruntime) +endfunction() diff --git a/docker/Dockerfile.jetson b/docker/Dockerfile.jetson index 7fffe7ed..d597ecc1 100644 --- a/docker/Dockerfile.jetson +++ b/docker/Dockerfile.jetson @@ -48,7 +48,8 @@ RUN --mount=target=/var/lib/apt/lists,type=cache,sharing=locked \ libmagick++-dev \ # needed for pytorch libopenblas-dev \ - ninja-build + ninja-build \ + libopencv-dev RUN pip3 install typing-extensions PyYAML cpplint diff --git a/docker/Makefile b/docker/Makefile index d244819a..97bb0f3b 100644 --- a/docker/Makefile +++ b/docker/Makefile @@ -42,5 +42,5 @@ run-jetson-cuda-check: # Note that to re-run cmake inside the container you'll need the really long CMake command # in the Dockerfile.jetson. jetson-develop: - cd .. && docker run -it --net=host --runtime=nvidia --platform=linux/arm64 --volume=./:/obcpp --device=/dev/ttyACM0 ghcr.io/tritonuas/obcpp:jetson /bin/bash + cd .. && docker run -it --net=host --runtime=nvidia --platform=linux/arm64 --volume=./:/obcpp --device=/dev/ttyTHS1 ghcr.io/tritonuas/obcpp:jetson /bin/bash # cd .. && docker run -it --net=host --runtime=nvidia --platform=linux/arm64 --volume=./:/obcpp ghcr.io/tritonuas/obcpp:jetson /bin/bash diff --git a/docker/sitl-compose.yml b/docker/sitl-compose.yml index 093734d6..2a24a192 100644 --- a/docker/sitl-compose.yml +++ b/docker/sitl-compose.yml @@ -8,6 +8,9 @@ services: # # - 5010:5010 not-stolen-israeli-code: image: ghcr.io/tritonuas/not-stolen-israeli-code:latest + volumes: + - ~/dev/tritonuas/not-stolen-israeli-code/backgrounds:/not-stolen-israeli-code/backgrounds + - ~/dev/tritonuas/not-stolen-israeli-code/emergent_target_pics:/not-stolen-israeli-code/emergent_target_pics ports: - 5069:5000 volumes: diff --git a/images/mapping/.gitkeep b/images/mapping/.gitkeep new file mode 100644 index 00000000..e69de29b diff --git a/include/core/mission_parameters.hpp b/include/core/mission_parameters.hpp index b8862081..f8d61a36 100644 --- a/include/core/mission_parameters.hpp +++ b/include/core/mission_parameters.hpp @@ -9,8 +9,9 @@ #include #include +// Make sure Airdrop type is known before it's used #include "pathing/cartesian.hpp" -#include "protos/obc.pb.h" +#include "protos/obc.pb.h" // Include for Airdrop definition #include "utilities/constants.hpp" #include "utilities/datatypes.hpp" @@ -29,16 +30,20 @@ class MissionParameters { Polygon getFlightBoundary(); Polygon getAirdropBoundary(); + Polygon getMappingBoundary(); Polyline getWaypoints(); - std::vector getAirdropBottles(); + // CHANGE: Return type is now std::vector + std::vector getAirdrops(); // Getters for multiple values // Use when need to get multiple values // Important to use this instead of the singular getters // to avoid race conditions - std::tuple> getConfig(); + // CHANGE: Tuple element type is now std::vector + std::tuple> getConfig(); // returns error string to be displayed back to the user + // Ensure CartesianConverter template parameter matches usage std::optional setMission(Mission, CartesianConverter); void saveToFile(std::string filename); @@ -50,13 +55,16 @@ class MissionParameters { Polygon flightBoundary; Polygon airdropBoundary; + Polygon mappingBoundary; Polyline waypoints; - std::vector bottles; + // CHANGE: Member variable type is now std::vector + std::vector airdrops; std::optional cached_mission{}; // internal function which doesn't apply the mutex - void _setBottle(Bottle bottle); + // CHANGE: Renamed function and changed parameter type to const Airdrop& + void _setAirdrop(const Airdrop& airdrop); }; #endif // INCLUDE_CORE_MISSION_PARAMETERS_HPP_ diff --git a/include/core/mission_state.hpp b/include/core/mission_state.hpp index 16391d24..f9ef78ea 100644 --- a/include/core/mission_state.hpp +++ b/include/core/mission_state.hpp @@ -3,6 +3,7 @@ #include #include +#include #include #include #include @@ -52,8 +53,8 @@ class MissionState { void setAirdropPath(const MissionPath& airdrop_path); MissionPath getAirdropPath(); - void markBottleAsDropped(BottleDropIndex bottle); - std::unordered_set getDroppedBottles(); + void markAirdropAsDropped(AirdropIndex airdrop); + std::unordered_set getDroppedAirdrops(); /* * Gets a locking reference to the underlying tick for the given tick subclass T. @@ -103,12 +104,15 @@ class MissionState { std::shared_ptr getCamera(); void setCamera(std::shared_ptr camera); - MissionParameters mission_params; // has its own mutex + // Getters and setters for mapping status. + bool getMappingIsDone(); + void setMappingIsDone(bool isDone); + MissionParameters mission_params; // has its own mutex OBCConfig config; - std::optional next_bottle_to_drop; + std::optional next_airdrop_to_drop; private: std::mutex converter_mut; @@ -124,8 +128,8 @@ class MissionState { std::mutex airdrop_path_mut; MissionPath airdrop_path; - std::mutex dropped_bottles_mut; - std::unordered_set dropped_bottles; + std::mutex dropped_airdrops_mut; + std::unordered_set dropped_airdrops; std::shared_ptr mav; std::shared_ptr airdrop; @@ -133,11 +137,13 @@ class MissionState { std::shared_ptr camera; std::mutex cv_mut; + // Represents a single detected target used in pipeline std::vector cv_detected_targets; // Gives an index into cv_detected_targets, and specifies that that bottle is matched // with the detected_target specified by the index - std::array cv_matches; + std::array cv_matches; + bool mappingIsDone; void _setTick(Tick* newTick); // does not acquire the tick_mut }; diff --git a/include/cv/aggregator.hpp b/include/cv/aggregator.hpp index ce71aca5..6b375b89 100644 --- a/include/cv/aggregator.hpp +++ b/include/cv/aggregator.hpp @@ -1,38 +1,55 @@ #ifndef INCLUDE_CV_AGGREGATOR_HPP_ #define INCLUDE_CV_AGGREGATOR_HPP_ -#include -#include -#include +#include +#include +#include #include +#include +#include #include -#include +#include #include +#include -#include "cv/utilities.hpp" #include "cv/pipeline.hpp" +#include "cv/utilities.hpp" +#include "protos/obc.pb.h" #include "utilities/constants.hpp" #include "utilities/lockptr.hpp" -struct CVResults { - std::vector detected_targets; - // mapping from bottle -> index into detected_targets - // (optional is none if we don't have a match yet) - std::unordered_map> matches; +struct AggregatedRun { + int run_id; // Unique integer ID for this pipeline run + cv::Mat annotatedImage; // The "big image" with bounding boxes drawn + std::vector bboxes; // All bounding boxes in that image + std::vector coords; // Matching lat-longs for each bounding box }; +struct CVResults { + std::vector runs; // Each pipeline invocation => 1 run +}; +struct MatchedResults { + std::unordered_map matched_airdrop; +}; class CVAggregator { public: explicit CVAggregator(Pipeline&& p); - ~CVAggregator(); + // Spawn a thread to run the pipeline on the given imageData void runPipeline(const ImageData& image); + // Lockable pointer to retrieve aggregator results LockPtr getResults(); + // Lockable pointer to retrieve matched results (after manual match) + LockPtr getMatchedResults(); + + // For the endpoint to reset the current list of structs + std::vector popAllRuns(); + private: void worker(ImageData image, int thread_num); @@ -41,10 +58,14 @@ class CVAggregator { std::mutex mut; int num_worker_threads; - // For when too many pipelines are active at the same time + // For when too many pipelines are active at once std::queue overflow_queue; + // Shared aggregator results std::shared_ptr results; + + // Shared matched results + std::shared_ptr matched_results; }; #endif // INCLUDE_CV_AGGREGATOR_HPP_ diff --git a/include/cv/classification.hpp b/include/cv/classification.hpp deleted file mode 100644 index 2bd46de4..00000000 --- a/include/cv/classification.hpp +++ /dev/null @@ -1,71 +0,0 @@ -#ifndef INCLUDE_CV_CLASSIFICATION_HPP_ -#define INCLUDE_CV_CLASSIFICATION_HPP_ - -#include - -#include -#include - -#include "cv/utilities.hpp" -#include "cv/segmentation.hpp" - - - -struct ClassificationResults { - // TODO: replace with protobuf structs instead of strings - std::string shape; - std::string shapeColor; - std::string character; - std::string characterColor; -}; - -// Classification is responsible for predicting characteristics about -// ground comptition targets. These characterisitcs include shape type, -// alphanumeric character type, shape color, and alphanumeric character color. -// Currently, the shape and character classifiers are implemented using a -// Convolutional Neural Network (CNN). -// The color classifier is implented using a K-nearest neigbors model (KNN) -// The implementation of the models themselves can be found here: -// https://github.com/tritonuas/taxonomy-101 -// In this class we will take the pretrained models and use them to make -// inferences. -class Classification { - public: - Classification() {} - explicit Classification(const std::string &shapeModelPath, \ - const std::string &characterModelPath, const std::string &colorModelPath); - // classify takes a cropped image of the target (saliency output) and - // two binary masks to represent which region of pixels correspond to - // shape and character respectivel (output of segmentation). Using this - // data, the shape type, character type, shape color and character color - // will be predicted. - ClassificationResults classify(cv::Mat croppedImage, cv::Mat shapeMask, cv::Mat characterMask); - - private: - torch::jit::script::Module shapeModel; - torch::jit::script::Module characterModel; - torch::jit::script::Module colorModel; - std::map index_to_shape; - - // classifyShape takes a cropped image of the target (output of saliency) - // and a binary mask (output of segmentation). The binary mask should - // represent which region of pixels correspond to the shape region of - // the target. - std::string classifyShape(cv::Mat croppedImage, cv::Mat shapeMask); - - // classifyShape takes a cropped image of the target (output of saliency) - // and a binary mask (output of segmentation). The binary mask should - // represent which region of pixels correspond to the character region of - // the target. - std::string classifyCharacter(cv::Mat croppedImage, cv::Mat characterMask); - - // classify the primary color of a region described by a binary mask. - // This can be used for finding both shape and character color since - // we will use the same algorithm to detect the primary color in - // whatever region the mask describes. All that changes is the mask - // that's passed in. - std::string classifyColor(cv::Mat croppedImage, cv::Mat mask); -}; - - -#endif // INCLUDE_CV_CLASSIFICATION_HPP_ diff --git a/include/cv/mapping.hpp b/include/cv/mapping.hpp index 8dcf8bdf..28cd3c38 100644 --- a/include/cv/mapping.hpp +++ b/include/cv/mapping.hpp @@ -13,10 +13,12 @@ // - finds new images in 'input_path' that haven't been processed yet // - splits them into chunks // - stitches each chunk, saves result on disk (not in memory) -// +// - frees memory after processing +// - **New:** Saves partial results in a new timestamped subfolder within the given run_subdir. // 2) secondPass: -// - scans for all chunk results in the output directory +// - scans for all chunk files in the timestamped run folder // - merges them into one final panorama +// - saves final panorama to the same folder // class Mapping { public: @@ -26,21 +28,29 @@ class Mapping { // First pass: // - Scans input_path for new images // - Processes them in overlapping chunks - // - Saves partial results to run_subdir + // - Saves partial results to a new timestamped folder inside run_subdir // - Frees memory for partial results + // Added parameter 'preprocess' to determine if images should be pre-processed. void firstPass(const std::string& input_path, const std::string& run_subdir, int chunk_size, - int overlap, cv::Stitcher::Mode mode, int max_dim); + int overlap, cv::Stitcher::Mode mode, int max_dim, bool preprocess = true); // Second pass: - // - Loads all partial chunk files from run_subdir + // - Loads all partial chunk files from the current timestamped run folder // - Merges them into one final panorama - // - Saves final panorama to run_subdir - void secondPass(const std::string& run_subdir, cv::Stitcher::Mode mode, int max_dim); + // - Saves final panorama to the same folder + // Added parameter 'preprocess' to determine if images should be pre-processed. + void secondPass(const std::string& run_subdir, cv::Stitcher::Mode mode, int max_dim, + bool preprocess = true); // Optional: reset tracking so you can reuse this object for new sets of images. // This clears the counters but does NOT delete your saved chunk images on disk. void reset(); + // Direct stitch: loads all images from input_path and stitches them into a single panorama + // Saves the result to output_path. This bypasses the two-pass chunking system. + void directStitch(const std::string& input_path, const std::string& output_path, + cv::Stitcher::Mode mode, int max_dim, bool preprocess = true); + private: // Helper: read & optionally downscale an image so its largest dimension <= max_dim static cv::Mat readAndResize(const cv::Mat& input, int max_dim); @@ -51,17 +61,19 @@ class Mapping { // Helper: performs a single global stitch on a vector of images. // Returns (Stitcher::Status, stitched_image). + // Added parameter 'preprocess' to determine if images should be pre-processed. static std::pair globalStitch(const std::vector& images, cv::Stitcher::Mode mode, - int max_dim); + int max_dim, bool preprocess); // Helper: generate a timestamp string like "2025-01-24-12-34-56" static std::string currentDateTimeStr(); // Internal: process a single chunk of images, store result on disk, - // and free them from memory + // and free them from memory. + // Added parameter 'preprocess' to determine if images should be pre-processed. void processChunk(const std::vector& chunk_images, const std::string& run_subdir, - cv::Stitcher::Mode mode, int max_dim); + cv::Stitcher::Mode mode, int max_dim, bool preprocess); private: // All recognized image filenames in the directory (sorted) @@ -75,6 +87,10 @@ class Mapping { // For naming the chunk files we produce int chunk_counter = 0; + + // New: the timestamped folder (inside the given run_subdir) for this run. + // This ensures that subsequent calls (e.g. secondPass) use only the current run's files. + std::string current_run_folder; }; #endif // INCLUDE_CV_MAPPING_HPP_ diff --git a/include/cv/matching.hpp b/include/cv/matching.hpp deleted file mode 100644 index b1c788df..00000000 --- a/include/cv/matching.hpp +++ /dev/null @@ -1,60 +0,0 @@ -#ifndef INCLUDE_CV_MATCHING_HPP_ -#define INCLUDE_CV_MATCHING_HPP_ - -#include -#include -#include - -#include -#include -#include - -#include -#include "cv/utilities.hpp" -#include "utilities/constants.hpp" -#include "utilities/datatypes.hpp" - -#include "protos/obc.pb.h" - -struct MatchResult { - MatchResult(BottleDropIndex index, double distance) - : bottleDropIndex{index}, distance{distance} {} - - BottleDropIndex bottleDropIndex; - double distance; -}; - -// Matching is used to match targets to a potential corresponding competition -// bottle drop objective. Before the misssion starts, the competition tells us -// the characteristics of the ground targets that we should drop bottles on. -// Using this information we know which targets to look for. Then, we can -// calculate how similar the targets we capture are to the ones the competition -// enumerates for the bottle drop. -// This is implemented using a Siamese Neural Netowrk that takes two images as -// input and outputs a similarity score. One of the input images will be the -// cropped target from saliency. The other input will be an artificially -// generated image that matches the description of one of the competition -// objectives. We will use something similar to not-stolen-israeli-code to do -// the generation (https://github.com/tritonuas/not-stolen-israeli-code). -// The matching siamese model is implemented in this repo: -// https://github.com/tritonuas/fraternal-targets -// -// One important note is that matching is a replacement for the segmentation -// and classification stages of the pipeline. Instead of outright guessing -// the properties of the targets (as with segmentation/classification) we can -// see how close it is to known objectives. -class Matching { - public: - Matching(std::array competitionObjectives, - std::vector> referenceImages, - const std::string &modelPath); - - MatchResult match(const CroppedTarget& croppedTarget); - - private: - std::array competitionObjectives; - std::vector> referenceFeatures; - torch::jit::script::Module torch_module; -}; - -#endif // INCLUDE_CV_MATCHING_HPP_ diff --git a/include/cv/pipeline.hpp b/include/cv/pipeline.hpp index 28d87dd9..42dffc64 100644 --- a/include/cv/pipeline.hpp +++ b/include/cv/pipeline.hpp @@ -1,67 +1,59 @@ #ifndef INCLUDE_CV_PIPELINE_HPP_ #define INCLUDE_CV_PIPELINE_HPP_ -#include -#include +#include +#include #include +#include +#include #include #include "camera/interface.hpp" -#include "cv/classification.hpp" #include "cv/localization.hpp" -#include "cv/matching.hpp" -#include "cv/saliency.hpp" -#include "cv/segmentation.hpp" +#include "cv/preprocess.hpp" #include "cv/utilities.hpp" +#include "cv/yolo.hpp" +#include "protos/obc.pb.h" // Processed image holds all predictions made concerning a given image. -// -// A single aerial image can have multiple targets that are matches with -// competition bottle assignments. -// At the same time, an aerial image can have other targets that are not matched -// with a bottle (or at least the pipeline is not confident in a match). struct PipelineResults { PipelineResults(ImageData imageData, std::vector targets) - : imageData{imageData}, targets{targets} {} + : imageData(std::move(imageData)), targets(std::move(targets)) {} + // This holds the final annotated image in imageData.DATA ImageData imageData; + + // This is the list of YOLO + localized detections std::vector targets; }; struct PipelineParams { - PipelineParams(std::array competitionObjectives, - std::vector> referenceImages, - std::string matchingModelPath, std::string segmentationModelPath, - std::string saliencyModelPath) - : competitionObjectives{competitionObjectives}, referenceImages{referenceImages}, - matchingModelPath{matchingModelPath}, segmentationModelPath{segmentationModelPath}, - saliencyModelPath{saliencyModelPath} {} - - std::array competitionObjectives; - std::vector> referenceImages; - std::string matchingModelPath; - std::string segmentationModelPath; - std::string saliencyModelPath; + // Added outputPath parameter with default empty string + explicit PipelineParams(std::string yoloModelPath, std::string outputPath = "", + bool do_preprocess = true) + : yoloModelPath{std::move(yoloModelPath)}, + outputPath(std::move(outputPath)), + do_preprocess(do_preprocess) {} + + std::string yoloModelPath; + bool do_preprocess; + std::string outputPath; }; -// Pipeline handles all infrastructure within the CV pipeline +// Pipeline handles YOLO + localization (and now optional preprocessing and output saving) class Pipeline { public: explicit Pipeline(const PipelineParams& p); - PipelineResults run(const ImageData& imageData); private: - Saliency detector; - - Matching matcher; - - Segmentation segmentor; - Classification classifier; - - ECEFLocalization ecefLocalizer; + std::unique_ptr yoloDetector; + // ECEFLocalization ecefLocalizer; GSDLocalization gsdLocalizer; + bool do_preprocess; // Flag to enable/disable preprocessing + Preprocess preprocessor; // Preprocess utility instance + std::string outputPath; // New member to hold output image path }; #endif // INCLUDE_CV_PIPELINE_HPP_ diff --git a/include/cv/preprocess.hpp b/include/cv/preprocess.hpp new file mode 100644 index 00000000..7b4b3362 --- /dev/null +++ b/include/cv/preprocess.hpp @@ -0,0 +1,20 @@ +#ifndef INCLUDE_CV_PREPROCESS_HPP_ +#define INCLUDE_CV_PREPROCESS_HPP_ + +#include + +// Preprocess chops off 20px from the right side of the image to +// account for the green bar from the camera +class Preprocess { + public: + Preprocess() = default; + + // Crops 20 pixels from the right side of the provided image. + // Returns a new cv::Mat containing the cropped image. + cv::Mat cropRight(const cv::Mat &image) const; + + private: + const int crop_pixels_ = 20; +}; + +#endif // INCLUDE_CV_PREPROCESS_HPP_ diff --git a/include/cv/saliency.hpp b/include/cv/saliency.hpp deleted file mode 100644 index fa3b0ab2..00000000 --- a/include/cv/saliency.hpp +++ /dev/null @@ -1,41 +0,0 @@ -#ifndef INCLUDE_CV_SALIENCY_HPP_ -#define INCLUDE_CV_SALIENCY_HPP_ -#include -#include -#include -#include -#include "cv/utilities.hpp" - -// Saliency is responsible for detecting targets within a full-size aerial image -// It can detect the presence of standard targets (with shape and character) -// as well as emergent targets (mannikin). It outputs the position of the -// targets within the image (using coordinates of a bounding box) and a -// classification between if a target is standard or emergent. Saliency is -// implemented as a convolutional neural network using the Faster RCNN -// model. The training/testing code for the model can be found here: -// https://github.com/tritonuas/garretts-new-lunchbox -class Saliency { - public: - // saliency takes a full-size aerial image and detects any potential - // targets within the image. The full-size image can have multiple - // targets which is why it returns a vector of targets. For each target, - // it will predict the location within the full-size image (using - // coordinates of bounding box) and a prediction of whether or not a - // target is emergent (mannikin) or not. - - explicit Saliency(std::string modelPath); // constructor with model path - - std::vector salience(cv::Mat image); // saliency function - - // helper functions - at::Tensor ToTensor(cv::Mat img, bool show_output, bool unsqueeze, int unsqueeze_dim); - at::Tensor transpose(at::Tensor tensor, c10::IntArrayRef dims); - std::vector ToInput(at::Tensor tensor_image); - std::vector extractTargets(c10::List listDetections, cv::Mat image); - - private: - std::string modelPath; // path to prediction model - torch::jit::script::Module module; // the loaded model -}; - -#endif // INCLUDE_CV_SALIENCY_HPP_ diff --git a/include/cv/segmentation.hpp b/include/cv/segmentation.hpp deleted file mode 100644 index 4078df6f..00000000 --- a/include/cv/segmentation.hpp +++ /dev/null @@ -1,45 +0,0 @@ -#ifndef INCLUDE_CV_SEGMENTATION_HPP_ -#define INCLUDE_CV_SEGMENTATION_HPP_ - -#include - -#include -#include -#include -#include -#include - -#include - -#include "cv/utilities.hpp" - -struct SegmentationResults { - cv::Mat shapeMask; - cv::Mat characterMask; -}; - -// Segmentation is the stage of the pipeline responsible for detecting the -// region of a cropped target that corresponds to the shape/character -// attributes. This is useful for simplifying the job of the classifcation model. -// Assuming that segmentation is accurate, classification only needs to look at -// the binary mask of the target's shape and character. This simplifies -// classification to a simple MNIST classification problem where we need to -// classify a black and white shape/character image. The training/testing code -// for segmentation models can be found here: -// https://github.com/tritonuas/hutzler-571 -class Segmentation { - public: - explicit Segmentation(const std::string &modelPath); - SegmentationResults segment(const CroppedTarget &target); - private: - torch::jit::script::Module module; -}; - -std::string get_image_type(const cv::Mat& img, bool more_info); -void show_image(cv::Mat& img, std::string title); -at::Tensor transpose(at::Tensor tensor, c10::IntArrayRef dims); -std::vector ToInput(at::Tensor tensor_image); -at::Tensor ToTensor(cv::Mat img, bool show_output, bool unsqueeze, int unsqueeze_dim); -cv::Mat ToCvImage(at::Tensor tensor); - -#endif // INCLUDE_CV_SEGMENTATION_HPP_ diff --git a/include/cv/utilities.hpp b/include/cv/utilities.hpp index 65ccd9cd..f32e99d4 100644 --- a/include/cv/utilities.hpp +++ b/include/cv/utilities.hpp @@ -1,12 +1,10 @@ #ifndef INCLUDE_CV_UTILITIES_HPP_ #define INCLUDE_CV_UTILITIES_HPP_ -#include - #include -#include "utilities/constants.hpp" #include "protos/obc.pb.h" +#include "utilities/constants.hpp" class Bbox { public: @@ -19,22 +17,14 @@ class Bbox { int height(); }; -struct CroppedTarget { - cv::Mat croppedImage; - Bbox bbox; - bool isMannikin; -}; - struct DetectedTarget { - DetectedTarget(GPSCoord coord, BottleDropIndex index, double match_distance, CroppedTarget crop) - :coord{coord}, likely_bottle{index}, match_distance{match_distance}, crop{crop} {} - - GPSCoord coord; - BottleDropIndex likely_bottle; - double match_distance; - CroppedTarget crop; + Bbox bbox; // The bounding box in pixel coords + GPSCoord coord; // The localized GPS coordinate + AirdropIndex likely_airdrop; + double match_distance; // Inverse confidence or other distance metric }; -cv::Mat crop(cv::Mat original, Bbox bbox); +// Helper function to crop out a bounding box from an image if you still need it +cv::Mat crop(const cv::Mat& original, const Bbox& bbox); #endif // INCLUDE_CV_UTILITIES_HPP_ diff --git a/include/cv/yolo.hpp b/include/cv/yolo.hpp new file mode 100644 index 00000000..1248df9f --- /dev/null +++ b/include/cv/yolo.hpp @@ -0,0 +1,97 @@ +#ifndef INCLUDE_CV_YOLO_HPP_ +#define INCLUDE_CV_YOLO_HPP_ + +#include + +#include +#include + +#include + + +/// Simple struct to store a detection result +struct Detection { + float x1; + float y1; + float x2; + float y2; + float confidence; + int class_id; +}; + +class YOLO { + public: + /** + * @brief Construct a new YOLO object + * + * @param modelPath Path to the YOLO .onnx file + * @param confThreshold Minimum confidence threshold for a detection + * @param inputWidth Width of the model input + * @param inputHeight Height of the model input + */ + YOLO(const std::string &modelPath, float confThreshold = 0.25f, int inputWidth = 640, + int inputHeight = 640); + + /** + * @brief Destroy the YOLO object + */ + ~YOLO(); + + /** + * @brief Perform inference on an input image. + * + * @param image Input image (cv::Mat in BGR format) + * @return std::vector A list of detections + */ + std::vector detect(const cv::Mat &image); + + /** + * @brief Draws and prints the given detections on the image. + * + * @param image The original image on which boxes and labels will be drawn + * @param detections The list of detections to visualize + */ + void drawAndPrintDetections(cv::Mat &image, const std::vector &detections); + + /** + * @brief Process an input image: detect objects, draw detections, and save the output image. + * + * @param image The input image (cv::Mat in BGR format) + * @param outputFile Path to save the output image. + */ + void processAndSaveImage(const cv::Mat &image, const std::string &outputFile); + + private: + /// Preprocess a cv::Mat to match the model's input shape and format + std::vector preprocess(const cv::Mat &image); + + /** + * @brief Resize + pad the image to maintain aspect ratio as typical YOLO does. + * + * @param src The original BGR image + * @param newWidth Target width (e.g. 640) + * @param newHeight Target height (e.g. 640) + * @param color Letterbox color fill (default 114 for typical YOLO) + * @return cv::Mat The letterboxed image of size [newHeight x newWidth] + */ + cv::Mat letterbox(const cv::Mat &src, int newWidth, int newHeight, + const cv::Scalar &color = cv::Scalar(114, 114, 114)); + + private: + Ort::Env env_; + Ort::Session *session_; + Ort::SessionOptions sessionOptions_; + + float confThreshold_; + int inputWidth_; + int inputHeight_; + float scale_ = 1.f; + int padTop_ = 0; + int padLeft_ = 0; + + // Input/Output node information + std::vector inputNames_; + std::vector outputNames_; +}; + +#endif // INCLUDE_CV_YOLO_HPP_ diff --git a/include/network/airdrop_client.hpp b/include/network/airdrop_client.hpp index e0bdee76..5758ab68 100644 --- a/include/network/airdrop_client.hpp +++ b/include/network/airdrop_client.hpp @@ -32,7 +32,7 @@ class AirdropClient { // Returns list of all the payloads we have not heard from for more than // `threshold` seconds, and includes how many seconds it has been since // we last heard from them. - std::list> + std::list> getLostConnections(std::chrono::milliseconds threshold); std::optional getMode(); @@ -48,7 +48,7 @@ class AirdropClient { std::future worker_future; // holds unix timestamp of the last heartbeat received from every payload - std::array last_heartbeat; + std::array last_heartbeat; // Get initial SET_MODE msg from payloads and set up workers void _establishConnection(); diff --git a/include/network/gcs_routes.hpp b/include/network/gcs_routes.hpp index d4b29c90..8387a5cd 100644 --- a/include/network/gcs_routes.hpp +++ b/include/network/gcs_routes.hpp @@ -4,25 +4,24 @@ #include #include -#include #include -#include #include +#include +#include -#include "utilities/logging.hpp" #include "core/mission_state.hpp" -#include "protos/obc.pb.h" -#include "utilities/serialize.hpp" #include "network/gcs_macros.hpp" -#include "ticks/tick.hpp" +#include "protos/obc.pb.h" #include "ticks/path_gen.hpp" - +#include "ticks/tick.hpp" +#include "utilities/logging.hpp" +#include "utilities/serialize.hpp" /* * GET /connection * --- * Returns information about the connection status of the OBC - * + * * 200 OK: Successfully retrieved data */ DEF_GCS_HANDLE(Get, connections); @@ -31,7 +30,7 @@ DEF_GCS_HANDLE(Get, connections); * GET /tick * --- * Returns the name of the current tick function, for display in the GCS control page - * + * * 200 OK: No problems occurred **/ DEF_GCS_HANDLE(Get, tick); @@ -40,22 +39,22 @@ DEF_GCS_HANDLE(Get, tick); * GET /mission * --- * Response includes the information stored in the MissionConfig as a JSON object. - * + * * { * TODO: fill in expected JSON output * } - * + * * 200 OK: no problems encountered */ DEF_GCS_HANDLE(Get, mission); /* * POST /mission - * + * * { * TODO: fill in the expected JSON format * } - * + * * TODO: reference protobuf class that encompasses the JSON * --- * Response is plain text that says whether posting was successful or not. @@ -73,7 +72,7 @@ DEF_GCS_HANDLE(Post, targets, locations); /* * GET /path/initial - * + * * 200 OK: path was previously generated and returned * 404 NOT FOUND: no path has been generated yet * TODO: determine if there are more errors we might encounter @@ -82,7 +81,7 @@ DEF_GCS_HANDLE(Get, path, initial); /* * GET /path/coverage - * + * * 200 OK: path was previously generated and returned * 404 NOT FOUND: no path has been generated yet * TODO: determine if there are more errors we might encounter @@ -91,10 +90,10 @@ DEF_GCS_HANDLE(Get, path, coverage); /* * GET /path/initial/new - * + * * --- * JSON output is the same as _getPathInitial - * + * * This request explicitly requests a newly generated initial path. In contrast, * GET /path/initial requests a cached path. The cached path is automatically generated * when all of the mission config information has been received. @@ -103,13 +102,13 @@ DEF_GCS_HANDLE(Get, path, initial, new); /* * POST /path/initial/validate - * + * * --- * Specifies that the operator is happy with the current generated/cached initial path. * Progresses beyond the PathGenTick step by setting a "initial_path_validated" * flag in the Mission State - * - * 200 OK: The initial path was generated, and is now validated + * + * 200 OK: The initial path was generated, and is now validated * 400 BAD REQUEST: There was no cached path to accept */ DEF_GCS_HANDLE(Post, path, initial, validate); @@ -179,14 +178,32 @@ DEF_GCS_HANDLE(Post, targets, matched); /** * POST /kill/kill/kill - * + * * TELLS THE PLANE TO CRASH ITSELF - * + * * ONLY CALL THIS IN EMERGENCY SITUATIONS */ DEF_GCS_HANDLE(Post, kill, kill, kill); +/** + * POST /camera/startstream + * --- + * Testing function to start the stream at a specific interval + * Expects a uint32 integer in the POST body as the interval to call startTakingPictures() + * + */ +DEF_GCS_HANDLE(Post, camera, startstream); + +/** + * POST /camera/endstream + * --- + * Testing function to end the stream. + * Takes the pictures in the camera queue and stores it in the config file path + */ +DEF_GCS_HANDLE(Post, camera, endstream); DEF_GCS_HANDLE(Get, oh, shit); +DEF_GCS_HANDLE(Get, tickstate); + #endif // INCLUDE_NETWORK_GCS_ROUTES_HPP_ diff --git a/include/network/mavlink.hpp b/include/network/mavlink.hpp index 0d7e2cef..d4def6b1 100644 --- a/include/network/mavlink.hpp +++ b/include/network/mavlink.hpp @@ -6,8 +6,8 @@ #include #include #include -#include #include +#include #include #include @@ -20,9 +20,9 @@ #include #include +#include "pathing/mission_path.hpp" #include "protos/obc.pb.h" #include "utilities/datatypes.hpp" -#include "pathing/mission_path.hpp" #include "utilities/obc_config.hpp" class MissionState; @@ -34,7 +34,7 @@ class MavlinkClient { * @param link link to the MAVLink connection ip port -> [protocol]://[ip]:[port] * example: * MavlinkClient("tcp://192.168.65.254:5762") - */ + */ explicit MavlinkClient(OBCConfig config); /* @@ -56,12 +56,12 @@ class MavlinkClient { * of the state, or if the initial path is empty, which will make it return false. This * should never happen due to how the state machine is set up, but it is there just in case. */ - bool uploadMissionUntilSuccess(std::shared_ptr state, - bool upload_geofence, const MissionPath& waypoints) const; + bool uploadMissionUntilSuccess(std::shared_ptr state, bool upload_geofence, + const MissionPath& waypoints) const; bool uploadGeofenceUntilSuccess(std::shared_ptr state) const; bool uploadWaypointsUntilSuccess(std::shared_ptr state, - const MissionPath& waypoints) const; + const MissionPath& waypoints) const; std::pair latlng_deg(); double altitude_agl_m(); @@ -82,6 +82,14 @@ class MavlinkClient { bool armAndHover(std::shared_ptr state); bool startMission(); + /* + * Triggers a relay on the ArduPilot + * @param relay_number The relay number to trigger (0-based, so RELAY2 = 1) + * @param state True to turn on, false to turn off + * @return True if successful, false otherwise + */ + bool triggerRelay(int relay_number, bool state); + void KILL_THE_PLANE_DO_NOT_CALL_THIS_ACCIDENTALLY(); private: diff --git a/include/pathing/static.hpp b/include/pathing/static.hpp index a405167e..ca22b8d1 100644 --- a/include/pathing/static.hpp +++ b/include/pathing/static.hpp @@ -16,7 +16,6 @@ #include "pathing/mission_path.hpp" #include "pathing/plotting.hpp" #include "pathing/tree.hpp" -#include "udp_squared/internal/enum.h" #include "utilities/constants.hpp" #include "utilities/datatypes.hpp" #include "utilities/rng.hpp" diff --git a/include/ticks/airdrop_approach.hpp b/include/ticks/airdrop_approach.hpp index 27094dc8..0261f9bc 100644 --- a/include/ticks/airdrop_approach.hpp +++ b/include/ticks/airdrop_approach.hpp @@ -1,14 +1,14 @@ #ifndef INCLUDE_TICKS_AIRDROP_APPROACH_HPP_ #define INCLUDE_TICKS_AIRDROP_APPROACH_HPP_ -#include #include +#include #include "ticks/tick.hpp" /* * Fly to aidrop points and communicate with airdrop mechanism. - * + * * See https://tritonuas.github.io/wiki/software/obc/tick_architecture/ticks/airdropapproach/ */ class AirdropApproachTick : public Tick { @@ -22,4 +22,6 @@ class AirdropApproachTick : public Tick { Tick* tick() override; }; +bool triggerAirdrop(std::shared_ptr mav, airdrop_t airdrop_index); + #endif // INCLUDE_TICKS_AIRDROP_APPROACH_HPP_ diff --git a/include/ticks/ids.hpp b/include/ticks/ids.hpp index 735c9c6e..7bd6a865 100644 --- a/include/ticks/ids.hpp +++ b/include/ticks/ids.hpp @@ -19,6 +19,7 @@ enum class TickID { MissionDone, ActiveTakeoff, WaitForTakeoff, + Refueling, }; #define _SET_TICK_ID_MAPPING(id) \ @@ -41,6 +42,7 @@ constexpr const char* TICK_ID_TO_STR(TickID id) { _SET_TICK_ID_MAPPING(MissionDone); _SET_TICK_ID_MAPPING(ActiveTakeoff); _SET_TICK_ID_MAPPING(WaitForTakeoff); + _SET_TICK_ID_MAPPING(Refueling); default: return "Unknown TickID"; } } diff --git a/include/ticks/manual_landing.hpp b/include/ticks/manual_landing.hpp index fdfb6c08..82f49a78 100644 --- a/include/ticks/manual_landing.hpp +++ b/include/ticks/manual_landing.hpp @@ -11,11 +11,14 @@ */ class ManualLandingTick : public Tick { public: - explicit ManualLandingTick(std::shared_ptr state); + explicit ManualLandingTick(std::shared_ptr state, Tick* next_tick); std::chrono::milliseconds getWait() const override; Tick* tick() override; + + private: + Tick* next_tick; }; #endif // INCLUDE_TICKS_MANUAL_LANDING_HPP_ diff --git a/include/ticks/mission_prep.hpp b/include/ticks/mission_prep.hpp index 047f85bb..0e5f0859 100644 --- a/include/ticks/mission_prep.hpp +++ b/include/ticks/mission_prep.hpp @@ -11,8 +11,6 @@ #include "cv/pipeline.hpp" #include "cv/aggregator.hpp" -#define NUMBOTTLES 5 - /* * Checks every second whether or not a valid mission has been uploaded. * Transitions to PathGenTick once it has been generated. @@ -27,10 +25,10 @@ class MissionPrepTick : public Tick { Tick* tick() override; private: - std::vector> - generateReferenceImages(std::array competitionObjectives); + std::vector> + generateReferenceImages(std::array competitionObjectives); - std::string getNotStolenRoute(const Bottle& target); + std::string getNotStolenRoute(const Airdrop& target); }; #endif // INCLUDE_TICKS_MISSION_PREP_HPP_ diff --git a/include/ticks/refueling.hpp b/include/ticks/refueling.hpp new file mode 100644 index 00000000..adef851c --- /dev/null +++ b/include/ticks/refueling.hpp @@ -0,0 +1,21 @@ +#ifndef INCLUDE_TICKS_REFUELING_HPP_ +#define INCLUDE_TICKS_REFUELING_HPP_ + +#include +#include + +#include "ticks/tick.hpp" + +/* + * Wait for the refueling process to finish. + */ +class RefuelingTick : public Tick { + public: + explicit RefuelingTick(std::shared_ptr state); + + std::chrono::milliseconds getWait() const override; + + Tick* tick() override; +}; + +#endif // INCLUDE_TICKS_REFUELING_HPP_ diff --git a/include/udp_squared b/include/udp_squared index ea3b9d26..af55bc81 160000 --- a/include/udp_squared +++ b/include/udp_squared @@ -1 +1 @@ -Subproject commit ea3b9d26740c092fe4b409e11cee0fa889a0701a +Subproject commit af55bc810b3b328f470218a7a91f5500fdf9a27e diff --git a/include/utilities/constants.hpp b/include/utilities/constants.hpp index de09e1f3..732a2a2b 100644 --- a/include/utilities/constants.hpp +++ b/include/utilities/constants.hpp @@ -50,7 +50,7 @@ const uint16_t WIND_COV = 231; const int DEFAULT_GCS_PORT = 5010; -const int NUM_AIRDROP_BOTTLES = 5; +const int NUM_AIRDROPS = 4; const char MISSION_CONFIG_PATH[] = "./mission-config.json"; const double TAKEOFF_ALTITUDE_M = 30.0; @@ -77,6 +77,7 @@ const std::chrono::milliseconds AUTO_LANDING_TICK_WAIT = std::chrono::millisecon const std::chrono::milliseconds MISSION_DONE_TICK_WAIT = std::chrono::milliseconds(100); const std::chrono::milliseconds ACTIVE_TAKEOFF_TICK_WAIT = std::chrono::milliseconds(100); const std::chrono::milliseconds WAIT_FOR_TAKEOFF_TICK_WAIT = std::chrono::milliseconds(100); +const std::chrono::milliseconds REFUELING_TICK_WAIT = std::chrono::milliseconds(100); const double EARTH_RADIUS_METERS = 6378137.0; diff --git a/include/utilities/datatypes.hpp b/include/utilities/datatypes.hpp index aa5b7373..28ec0882 100644 --- a/include/utilities/datatypes.hpp +++ b/include/utilities/datatypes.hpp @@ -92,9 +92,7 @@ using Polyline = std::vector; using GPSProtoVec = google::protobuf::RepeatedPtrField; -std::string ODLCShapeToString(const ODLCShape& shape); - -std::string ODLCColorToString(const ODLCColor& color); +std::string ODLCObjectsToString(const ODLCObjects& object); #endif // INCLUDE_UTILITIES_DATATYPES_HPP_ diff --git a/include/utilities/obc_config.hpp b/include/utilities/obc_config.hpp index 7836cb18..0bc50d37 100644 --- a/include/utilities/obc_config.hpp +++ b/include/utilities/obc_config.hpp @@ -1,12 +1,13 @@ #ifndef INCLUDE_UTILITIES_OBC_CONFIG_HPP_ #define INCLUDE_UTILITIES_OBC_CONFIG_HPP_ -#include +#include #include -#include -#include #include -#include +#include +#include +#include + #include "udp_squared/internal/enum.h" #include "utilities/constants.hpp" #include "utilities/datatypes.hpp" @@ -31,26 +32,24 @@ struct NetworkConfig { struct TakeoffConfig { float altitude_m; + int payload_size; }; struct CVConfig { - std::string matching_model_dir; - std::string segmentation_model_dir; - std::string saliency_model_dir; + std::string yolo_model_dir; std::string not_stolen_addr; uint16_t not_stolen_port; }; namespace PointFetchMethod { - enum class Enum { - NONE, // check RRT against every node (path optimal, but incredibly slow) - RANDOM, // check ~k randomly sampled nodes from the tree. - NEAREST // check ~$p$ nodes closest to the sampled node (best performance/time ratio from - // rudimentary testing) - }; - CONFIG_VARIANT_MAPPING_T(Enum) MAPPINGS = { - {"none", Enum::NONE}, {"random", Enum::RANDOM}, {"nearest", Enum::NEAREST} - }; +enum class Enum { + NONE, // check RRT against every node (path optimal, but incredibly slow) + RANDOM, // check ~k randomly sampled nodes from the tree. + NEAREST // check ~$p$ nodes closest to the sampled node (best performance/time ratio from + // rudimentary testing) +}; +CONFIG_VARIANT_MAPPING_T(Enum) +MAPPINGS = {{"none", Enum::NONE}, {"random", Enum::RANDOM}, {"nearest", Enum::NEAREST}}; }; // namespace PointFetchMethod struct DubinsConfig { @@ -65,16 +64,12 @@ struct RRTConfig { PointFetchMethod::Enum point_fetch_method; bool allowed_to_skip_waypoints; // if true, will skip waypoints if it can not connect after 1 // RRT iteration + bool generate_deviations; }; namespace AirdropCoverageMethod { - enum class Enum { - HOVER, - FORWARD - }; - CONFIG_VARIANT_MAPPING_T(Enum) MAPPINGS = { - {"hover", Enum::HOVER}, {"forward", Enum::FORWARD} - }; +enum class Enum { HOVER, FORWARD }; +CONFIG_VARIANT_MAPPING_T(Enum) MAPPINGS = {{"hover", Enum::HOVER}, {"forward", Enum::FORWARD}}; }; // namespace AirdropCoverageMethod struct AirdropCoverageConfig { @@ -93,18 +88,13 @@ struct AirdropCoverageConfig { }; namespace AirdropDropMethod { - enum class Enum { - GUIDED, - UNGUIDED - }; - CONFIG_VARIANT_MAPPING_T(Enum) MAPPINGS = { - {"guided", Enum::GUIDED}, {"unguided", Enum::UNGUIDED} - }; -}; +enum class Enum { GUIDED, UNGUIDED }; +CONFIG_VARIANT_MAPPING_T(Enum) MAPPINGS = {{"guided", Enum::GUIDED}, {"unguided", Enum::UNGUIDED}}; +}; // namespace AirdropDropMethod struct AirdropApproachConfig { AirdropDropMethod::Enum drop_method; - std::unordered_set bottle_ids; + std::unordered_set airdrop_ids; double drop_angle_rad; double drop_altitude_m; double guided_drop_distance_m; diff --git a/protos b/protos index 78471fb7..df972e41 160000 --- a/protos +++ b/protos @@ -1 +1 @@ -Subproject commit 78471fb7af829bc86bc80427f15b238ed1f1f8d4 +Subproject commit df972e412ca1e93b2d799bc906d7a906e9b62505 diff --git a/scripts/install-protos.sh b/scripts/install-protos.sh index ebc6ffd2..11d0865a 100755 --- a/scripts/install-protos.sh +++ b/scripts/install-protos.sh @@ -1,5 +1,9 @@ #!/bin/sh +# This overrides protos and always pull from main. +# Manually go to the protos folder and checkout to the desired branch +# Then cd to build and run the last line + # This script will be run from inside the build directory because it is executed from # CMake, which we run from inside the build directory if [ "${GITHUB_ACTIONS}" != "true" ]; then diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index e3309f9b..1ab3093a 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -53,6 +53,7 @@ target_add_mavsdk(${PROJECT_NAME}) target_add_opencv(${PROJECT_NAME}) target_add_httplib(${PROJECT_NAME}) target_add_loguru(${PROJECT_NAME}) +target_add_onnxruntime(${PROJECT_NAME}) # for some reason calling target_add_imagemagick here conflicts with, so we are including/linking without the function call # target_add_imagemagick(${PROJECT_NAME}) target_include_directories(${PROJECT_NAME} PRIVATE ${ImageMagick_INCLUDE_DIRS}) diff --git a/src/camera/interface.cpp b/src/camera/interface.cpp index 80c7fa94..06813790 100644 --- a/src/camera/interface.cpp +++ b/src/camera/interface.cpp @@ -58,6 +58,10 @@ std::optional queryMavlinkImageTelemetry( double pitch_deg = mavlinkClient->pitch_deg(); double roll_deg = mavlinkClient->roll_deg(); + std::cout << "lat_deg" << lat_deg << std::endl; + std::cout << "long deg" << lon_deg << std::endl; + std::cout << "altitude" << altitude_agl_m << std::endl; + return ImageTelemetry { .latitude_deg = lat_deg, .longitude_deg = lon_deg, diff --git a/src/camera/mock.cpp b/src/camera/mock.cpp index a125377d..64d2c758 100644 --- a/src/camera/mock.cpp +++ b/src/camera/mock.cpp @@ -30,6 +30,8 @@ MockCamera::MockCamera(CameraConfig config) : CameraInterface(config) { 0, telemetry); this->mock_images.push_back(img_data); + } else { + LOG_F(ERROR, "IMG DIRECTORY IS EMPTY | RIP CAT"); } }); } @@ -106,6 +108,7 @@ std::optional MockCamera::takePicture(const std::chrono::milliseconds // if we can't find corresonding telemtry json, just query mavlink if (!img_data.TELEMETRY.has_value()) { + LOG_F(ERROR, "no image json value"); img_data.TELEMETRY = queryMavlinkImageTelemetry(mavlinkClient); } diff --git a/src/core/CMakeLists.txt b/src/core/CMakeLists.txt index 9c4bac18..f66e8303 100644 --- a/src/core/CMakeLists.txt +++ b/src/core/CMakeLists.txt @@ -35,6 +35,7 @@ target_add_httplib(${LIB_NAME}) target_add_mavsdk(${LIB_NAME}) target_add_matplot(${LIB_NAME}) target_add_loguru(${LIB_NAME}) +target_add_onnxruntime(${LIB_NAME}) target_include_directories(${LIB_NAME} PRIVATE ${ImageMagick_INCLUDE_DIRS}) target_link_libraries(${LIB_NAME} PRIVATE -Wl,--copy-dt-needed-entries ${ImageMagick_LIBRARIES}) \ No newline at end of file diff --git a/src/core/mission_parameters.cpp b/src/core/mission_parameters.cpp index 3ffed86a..ac057fef 100644 --- a/src/core/mission_parameters.cpp +++ b/src/core/mission_parameters.cpp @@ -1,18 +1,18 @@ +#include "core/mission_parameters.hpp" + #include #include -#include #include +#include +#include #include #include -#include -#include "core/mission_parameters.hpp" +#include "pathing/cartesian.hpp" +#include "protos/obc.pb.h" #include "utilities/constants.hpp" #include "utilities/datatypes.hpp" #include "utilities/locks.hpp" -#include "pathing/cartesian.hpp" - -#include "protos/obc.pb.h" // TODO: Log out mission config after it has been set @@ -20,21 +20,19 @@ MissionParameters::MissionParameters() { // Bottle updates work by finding the bottle already in the list // by index and setting its values to the updated values, so we // need to initialize placeholder values in the bottles vector - Bottle bottleA; - bottleA.set_index(BottleDropIndex::A); - Bottle bottleB; - bottleB.set_index(BottleDropIndex::B); - Bottle bottleC; - bottleC.set_index(BottleDropIndex::C); - Bottle bottleD; - bottleD.set_index(BottleDropIndex::D); - Bottle bottleE; - bottleE.set_index(BottleDropIndex::E); - this->bottles.push_back(bottleA); - this->bottles.push_back(bottleB); - this->bottles.push_back(bottleC); - this->bottles.push_back(bottleD); - this->bottles.push_back(bottleE); + Airdrop airdropA; + airdropA.set_index(AirdropIndex::Kaz); + Airdrop airdropB; + airdropB.set_index(AirdropIndex::Kimi); + Airdrop airdropC; + airdropC.set_index(AirdropIndex::Chris); + Airdrop airdropD; + airdropD.set_index(AirdropIndex::Daniel); + // This part is now correct because this->airdrops is std::vector + this->airdrops.push_back(airdropA); + this->airdrops.push_back(airdropB); + this->airdrops.push_back(airdropC); + this->airdrops.push_back(airdropD); } MissionParameters::MissionParameters(std::string filename) { @@ -43,49 +41,52 @@ MissionParameters::MissionParameters(std::string filename) { Polygon MissionParameters::getFlightBoundary() { ReadLock lock(this->mut); - return this->flightBoundary; } Polygon MissionParameters::getAirdropBoundary() { ReadLock lock(this->mut); - return this->airdropBoundary; } -Polyline MissionParameters::getWaypoints() { +Polygon MissionParameters::getMappingBoundary() { ReadLock lock(this->mut); + return this->mappingBoundary; +} +Polyline MissionParameters::getWaypoints() { + ReadLock lock(this->mut); return this->waypoints; } -std::vector MissionParameters::getAirdropBottles() { +std::vector MissionParameters::getAirdrops() { ReadLock lock(this->mut); - - return this->bottles; + return this->airdrops; } -std::tuple> MissionParameters::getConfig() { +std::tuple> +MissionParameters::getConfig() { ReadLock lock(this->mut); - return std::make_tuple( - this->flightBoundary, this->airdropBoundary, this->waypoints, this->bottles); + return std::make_tuple(this->flightBoundary, this->airdropBoundary, + this->mappingBoundary, this->waypoints, + this->airdrops); } -void MissionParameters::_setBottle(Bottle bottle) { - // Go until you find the bottle that has the same index, and replace all values - for (auto& curr_bottle : this->bottles) { - if (curr_bottle.index() == bottle.index()) { - curr_bottle = Bottle(bottle); +void MissionParameters::_setAirdrop(const Airdrop& airdrop) { + // Go until you find the airdrop that has the same index, and replace all values + for (auto& curr_airdrop : this->airdrops) { // existing_airdrop is Airdrop& + // Compare index from the parameter (Airdrop object) + if (curr_airdrop.index() == airdrop.index()) { + // Assign the whole Airdrop object + curr_airdrop = Airdrop(airdrop); break; } } } - std::optional MissionParameters::setMission( - Mission mission, CartesianConverter cconverter -) { + Mission mission, CartesianConverter cconverter) { WriteLock lock(this->mut); std::string err; @@ -100,6 +101,11 @@ std::optional MissionParameters::setMission( if (mission.airdropboundary().size() < 3) { err += "Airdrop boundary must have at least 3 coordinates."; } + + if (mission.mappingboundary().size() < 3) { + err += "Mapping boundary must have at least 3 coordinates."; + } + if (!err.empty()) { return err; } @@ -107,9 +113,10 @@ std::optional MissionParameters::setMission( this->cached_mission = mission; this->flightBoundary = cconverter.toXYZ(mission.flightboundary()); this->airdropBoundary = cconverter.toXYZ(mission.airdropboundary()); + this->mappingBoundary = cconverter.toXYZ(mission.mappingboundary()); this->waypoints = cconverter.toXYZ(mission.waypoints()); - for (auto bottle : mission.bottleassignments()) { - this->_setBottle(bottle); + for (const auto& airdrop : mission.airdropassignments()) { // Use const& for efficiency + this->_setAirdrop(airdrop); } return {}; diff --git a/src/core/mission_state.cpp b/src/core/mission_state.cpp index d9ec8870..6e0e1b7f 100644 --- a/src/core/mission_state.cpp +++ b/src/core/mission_state.cpp @@ -99,14 +99,14 @@ MissionPath MissionState::getAirdropPath() { return this->airdrop_path; } -void MissionState::markBottleAsDropped(BottleDropIndex bottle) { - Lock lock(this->dropped_bottles_mut); - this->dropped_bottles.insert(bottle); +void MissionState::markAirdropAsDropped(AirdropIndex airdrop) { + Lock lock(this->dropped_airdrops_mut); + this->dropped_airdrops.insert(airdrop); } -std::unordered_set MissionState::getDroppedBottles() { - Lock lock(this->dropped_bottles_mut); - return this->dropped_bottles; +std::unordered_set MissionState::getDroppedAirdrops() { + Lock lock(this->dropped_airdrops_mut); + return this->dropped_airdrops; } std::shared_ptr MissionState::getMav() { @@ -140,3 +140,11 @@ std::shared_ptr MissionState::getCamera() { void MissionState::setCamera(std::shared_ptr camera) { this->camera = camera; } + +bool MissionState::getMappingIsDone() { + return this->mappingIsDone; +} + +void MissionState::setMappingIsDone(bool isDone) { + this->mappingIsDone = isDone; +} diff --git a/src/cv/CMakeLists.txt b/src/cv/CMakeLists.txt index 87e34c2c..afc45765 100644 --- a/src/cv/CMakeLists.txt +++ b/src/cv/CMakeLists.txt @@ -4,12 +4,11 @@ set(FILES aggregator.cpp hdr.cpp localization.cpp - matching.cpp pipeline.cpp - saliency.cpp - segmentation.cpp utilities.cpp mapping.cpp + yolo.cpp + preprocess.cpp ) set(LIB_DEPS @@ -37,6 +36,7 @@ target_add_httplib(${LIB_NAME}) target_add_mavsdk(${LIB_NAME}) target_add_matplot(${LIB_NAME}) target_add_loguru(${LIB_NAME}) +target_add_onnxruntime(${LIB_NAME}) target_include_directories(${LIB_NAME} PRIVATE ${ImageMagick_INCLUDE_DIRS}) target_link_libraries(${LIB_NAME} PRIVATE -Wl,--copy-dt-needed-entries ${ImageMagick_LIBRARIES}) \ No newline at end of file diff --git a/src/cv/aggregator.cpp b/src/cv/aggregator.cpp index 88f4a6d2..49e5a786 100644 --- a/src/cv/aggregator.cpp +++ b/src/cv/aggregator.cpp @@ -1,92 +1,121 @@ #include "cv/aggregator.hpp" -#include "utilities/locks.hpp" + #include "utilities/constants.hpp" -#include "utilities/logging.hpp" #include "utilities/lockptr.hpp" +#include "utilities/locks.hpp" +#include "utilities/logging.hpp" -CVAggregator::CVAggregator(Pipeline&& p): pipeline{p} { +CVAggregator::CVAggregator(Pipeline&& p) : pipeline(std::move(p)) { this->num_worker_threads = 0; this->results = std::make_shared(); - // set each bottle to be initially unmatched - this->results->matches[BottleDropIndex::A] = {}; - this->results->matches[BottleDropIndex::B] = {}; - this->results->matches[BottleDropIndex::C] = {}; - this->results->matches[BottleDropIndex::D] = {}; - this->results->matches[BottleDropIndex::E] = {}; + this->matched_results = std::make_shared(); + + AirdropTarget dummy; // Create one dummy template + + // Configure the coordinate part of the dummy + GPSCoord* coord_in_dummy = dummy.mutable_coordinate(); // Let dummy own its coordinate + coord_in_dummy->set_altitude(0.0); + coord_in_dummy->set_latitude(0.0); + coord_in_dummy->set_longitude(0.0); + + dummy.set_object(ODLCObjects::Undefined); // Set common properties once + + // Now assign copies, each with its specific index + dummy.set_index(AirdropIndex::Kaz); + this->matched_results->matched_airdrop[AirdropIndex::Kaz] = dummy; + + dummy.set_index(AirdropIndex::Kimi); + this->matched_results->matched_airdrop[AirdropIndex::Kimi] = dummy; + + dummy.set_index(AirdropIndex::Chris); + this->matched_results->matched_airdrop[AirdropIndex::Chris] = dummy; + + dummy.set_index(AirdropIndex::Daniel); + this->matched_results->matched_airdrop[AirdropIndex::Daniel] = dummy; } + CVAggregator::~CVAggregator() {} LockPtr CVAggregator::getResults() { return LockPtr(this->results, &this->mut); } +LockPtr CVAggregator::getMatchedResults() { + return LockPtr(this->matched_results, &this->mut); +} + void CVAggregator::runPipeline(const ImageData& image) { Lock lock(this->mut); - if (this->num_worker_threads > MAX_CV_PIPELINES) { - LOG_F(WARNING, "Tried to spawn more than %ld CVAggregator workers at one time.", - MAX_CV_PIPELINES); + if (this->num_worker_threads >= MAX_CV_PIPELINES) { + // If we have too many running workers, just queue the new image + LOG_F(WARNING, "Too many CVAggregator workers (%d). Pushing to overflow queue...", + this->num_worker_threads); this->overflow_queue.push(image); - LOG_F(WARNING, "CVAggregator overflow queue at size %ld", this->overflow_queue.size()); + LOG_F(WARNING, "Overflow queue size is now %ld", this->overflow_queue.size()); return; } - static int thread_num = 0; - this->num_worker_threads++; - std::thread worker(&CVAggregator::worker, this, image, ++thread_num); - worker.detach(); + static int thread_counter = 0; + ++this->num_worker_threads; + std::thread worker_thread(&CVAggregator::worker, this, image, ++thread_counter); + worker_thread.detach(); // We don’t need to join in the caller } +static std::atomic global_run_id{0}; + void CVAggregator::worker(ImageData image, int thread_num) { loguru::set_thread_name(("cv worker " + std::to_string(thread_num)).c_str()); - LOG_F(INFO, "New CVAggregator worker spawned."); + LOG_F(INFO, "New CVAggregator worker #%d spawned.", thread_num); while (true) { - auto results = this->pipeline.run(image); - - Lock lock(this->mut); - for (auto curr_target : results.targets) { - // this->detected_targets.size() will end up being the index of the - // newly inserted target, once we insert it after the if/else - size_t detected_target_index = this->results->detected_targets.size(); - - std::optional curr_match_idx = - this->results->matches[curr_target.likely_bottle]; - if (!curr_match_idx.has_value()) { - LOG_F(INFO, "Made first match between target %ld and bottle %d", - detected_target_index, curr_target.likely_bottle); - - this->results->matches[curr_target.likely_bottle] = detected_target_index; - } else { - auto curr_match = this->results->detected_targets[curr_match_idx.value()]; - - if (curr_match.match_distance > curr_target.match_distance) { - LOG_F(INFO, - "Swapping match on bottle %d from target %ld -> %ld (distance %f -> %f)", - static_cast(curr_match.likely_bottle), - this->results->matches[curr_match.likely_bottle].value_or(0), - detected_target_index, - curr_match.match_distance, - curr_target.match_distance); - - this->results->matches[curr_match.likely_bottle] = detected_target_index; - } - } + // 1) Run the pipeline + auto pipeline_results = this->pipeline.run(image); + + // 2) Build ONE run for all detections in that pipeline output + AggregatedRun run; + run.run_id = global_run_id.fetch_add(1); + run.annotatedImage = pipeline_results.imageData.DATA.clone(); + run.bboxes.reserve(pipeline_results.targets.size()); + run.coords.reserve(pipeline_results.targets.size()); + + for (auto& det : pipeline_results.targets) { + run.bboxes.push_back(det.bbox); + run.coords.push_back(det.coord); + } - // do this last so we can still meaningfully access local var curr_target - // before this line - this->results->detected_targets.push_back(std::move(curr_target)); + { + Lock lock(this->mut); + this->results->runs.push_back(std::move(run)); } - if (this->overflow_queue.empty()) { - break; + // 3) If no more queued images, break + { + Lock lock(this->mut); + if (this->overflow_queue.empty()) { + break; + } + image = std::move(this->overflow_queue.front()); + this->overflow_queue.pop(); } + } - image = std::move(this->overflow_queue.front()); - this->overflow_queue.pop(); + // 4) Mark ourselves as finished + { + Lock lock(this->mut); + LOG_F(INFO, "CVAggregator worker #%d terminating. Active threads: %d -> %d", thread_num, + this->num_worker_threads, this->num_worker_threads - 1); + --this->num_worker_threads; } +} - LOG_F(INFO, "CVAggregator worker terminating."); - this->num_worker_threads--; +// Empty list (for endpoint) +std::vector CVAggregator::popAllRuns() { + Lock lock(this->mut); + // Move out everything + std::vector out = std::move(this->results->runs); + // Now results->runs is empty + this->results->runs.clear(); + return out; } diff --git a/src/cv/classification.cpp b/src/cv/classification.cpp deleted file mode 100644 index 5d61fb2a..00000000 --- a/src/cv/classification.cpp +++ /dev/null @@ -1,109 +0,0 @@ -#include "cv/classification.hpp" - -Classification::Classification(const std::string &shapeModelPath, \ -const std::string &characterModelPath, const std::string &colorModelPath) { - auto device = torch::kCPU; - if (torch::cuda::is_available()) { - device = torch::kCUDA; - } - - try { - this->shapeModel = torch::jit::load(shapeModelPath); - this->shapeModel.to(device); - } - catch (const c10::Error& e) { - std::cerr << "ERROR: could not load the shape model\n"; - throw; - } - - try { - this->characterModel = torch::jit::load(characterModelPath); - this->characterModel.to(device); - } - catch (const c10::Error& e) { - std::cerr << "ERROR: could not load the character model\n"; - throw; - } - - this->index_to_shape = { - { 1, "CIRCLE"}, - { 2, "SEMICIRCLE"}, - { 3, "QUARTER_CIRCLE"}, - { 4, "TRIANGLE"}, - { 5, "SQUARE"}, - { 6, "RECTANGLE"}, - { 7, "TRAPEZOID"}, - { 8, "PENTAGON"}, - { 9, "HEXAGON"}, - { 10, "HEPTAGON"}, - { 11, "OCTAGON"}, - { 12, "STAR"}, - { 13, "CROSS"} - }; - - /* We are not using color model for now */ - // try { - // this->colorModel = torch::jit::load(colorModelPath); - // this->colorModel.to(device); - // } - // catch (const c10::Error& e) { - // std::cerr << "ERROR: could not load the color model\n"; - // throw; - // } -} - -std::string Classification::classifyShape(cv::Mat croppedImage, cv::Mat shapeMask) { - // Prepare cropped image tensor - at::Tensor img_tensor = ToTensor(croppedImage, false, false, 0); - img_tensor = img_tensor.toType(c10::kFloat).div(255); - img_tensor = transpose(img_tensor, { (2), (0), (1) }); - - // Prepare mask tensor - at::Tensor mask_tensor = torch::from_blob(shapeMask.data, \ - { shapeMask.rows, shapeMask.cols}, at::kFloat); - mask_tensor = mask_tensor.expand({3, mask_tensor.sizes()[0], mask_tensor.sizes()[1]}); - - // multiply mask tensor with cropped image tensor - mask_tensor = mask_tensor*img_tensor; - mask_tensor.unsqueeze_(0); - - at::Tensor output = (this->shapeModel.forward(ToInput(mask_tensor))).toTensor(); - auto index = torch::argmax(output).item(); - return this->index_to_shape[index + 1]; -} - -std::string Classification::classifyCharacter(cv::Mat croppedImage, cv::Mat characterMask) { - // Prepare cropped image tensor - at::Tensor img_tensor = ToTensor(croppedImage, false, false, 0); - img_tensor = img_tensor.toType(c10::kFloat).div(255); - img_tensor = transpose(img_tensor, { (2), (0), (1) }); - - // Prepare mask tensor - at::Tensor mask_tensor = torch::from_blob(characterMask.data, \ - { characterMask.rows, characterMask.cols}, at::kFloat); - mask_tensor = mask_tensor.expand({3, mask_tensor.sizes()[0], mask_tensor.sizes()[1]}); - - // multiply mask tensor with cropped image tensor - mask_tensor = mask_tensor*img_tensor; - mask_tensor.unsqueeze_(0); - - at::Tensor output = (this->characterModel.forward(ToInput(mask_tensor))).toTensor(); - auto index = torch::argmax(output).item(); - if (index < 26) { - return std::string(1, 'A' + index); - } else { - return std::to_string(index - 26); - } -} - -std::string Classification::classifyColor(cv::Mat croppedImage, cv::Mat mask) { - return nullptr; -} - -ClassificationResults Classification::classify(cv::Mat croppedImage, cv::Mat shapeMask, \ -cv::Mat characterMask) { - ClassificationResults results; - results.character = this->classifyCharacter(croppedImage, characterMask); - results.shape = this->classifyShape(croppedImage, shapeMask); - return results; -} diff --git a/src/cv/localization.cpp b/src/cv/localization.cpp index 266e3e38..8574e310 100644 --- a/src/cv/localization.cpp +++ b/src/cv/localization.cpp @@ -38,7 +38,8 @@ ECEFLocalization::ECEFCoordinates ECEFLocalization::ENUtoECEF( return target; } -// Converts ECEF coordinates to GPS coordinates using Heikkinen's procedure +// Converts ECEF cooordinates to GPS coordinates using Heikkinen's procedure +// Dont use. Shit doesnt work :sob: GPSCoord ECEFLocalization::ECEFtoGPS(ECEFCoordinates ecef) { GPSCoord gps; double a = EARTH_RADIUS_METERS; @@ -128,6 +129,8 @@ GPSCoord GSDLocalization::localize(const ImageTelemetry& telemetry, const Bbox& // double GSD = (SENSOR_WIDTH * (telemetry.altitude_agl_m * 1000)) // / (FOCAL_LENGTH_MM * IMG_WIDTH_PX); + std::cout << "Pre-localization: " << telemetry.latitude_deg << " " << telemetry.longitude_deg; + double GSD = (ANGLE_OF_VIEW_RATIO * (telemetry.altitude_agl_m * 1000) /IMG_WIDTH_PX); diff --git a/src/cv/mapping.cpp b/src/cv/mapping.cpp index 147bc9f9..784549bd 100644 --- a/src/cv/mapping.cpp +++ b/src/cv/mapping.cpp @@ -11,8 +11,11 @@ #include #include + +#include "cv/preprocess.hpp" // include the Preprocess header #include "opencv2/opencv.hpp" #include "opencv2/stitching.hpp" +#include "utilities/logging.hpp" namespace fs = std::filesystem; @@ -50,29 +53,35 @@ std::vector> Mapping::chunkListWithOverlap(int num_images, int return chunks; } +// ----------------------------------------------------------------------------- +// Updated: globalStitch() now simply clones its images because they have already been pre-processed +// ----------------------------------------------------------------------------- std::pair Mapping::globalStitch(const std::vector& images, - cv::Stitcher::Mode mode, - int max_dim) { - // Create stitcher + cv::Stitcher::Mode mode, int max_dim, + bool /*preprocess*/) { cv::Ptr stitcher = cv::Stitcher::create(mode); - // Resize all input images - std::vector resized; - resized.reserve(images.size()); + std::vector processed; + processed.reserve(images.size()); + // Simply clone images – they are assumed to be preprocessed already. for (const auto& im : images) { - cv::Mat tmp = readAndResize(im, max_dim); - if (!tmp.empty()) { - resized.push_back(tmp); - } + processed.push_back(im.clone()); } - if (resized.size() < 2) { + if (processed.size() < 2) { + // Not enough images to stitch return {cv::Stitcher::ERR_NEED_MORE_IMGS, cv::Mat()}; } - // Perform stitching cv::Mat stitched; - cv::Stitcher::Status status = stitcher->stitch(resized, stitched); + cv::Stitcher::Status status; + try { + status = stitcher->stitch(processed, stitched); + } catch (const cv::Exception& e) { + std::cerr << "[globalStitch] OpenCV exception: " << e.what() << "\n"; + return {cv::Stitcher::ERR_CAMERA_PARAMS_ADJUST_FAIL, cv::Mat()}; + } + return {status, stitched}; } @@ -90,15 +99,15 @@ std::string Mapping::currentDateTimeStr() { // processChunk() // ----------------------------------------------------------------------------- void Mapping::processChunk(const std::vector& chunk_images, const std::string& run_subdir, - cv::Stitcher::Mode mode, int max_dim) { + cv::Stitcher::Mode mode, int max_dim, bool preprocess) { if (chunk_images.empty()) return; chunk_counter++; - std::cout << "\nProcessing chunk #" << chunk_counter << " with " << chunk_images.size() - << " images.\n"; + LOG_S(INFO) << "Processing chunk #" << chunk_counter << " with " << chunk_images.size() + << " images.\n"; - // Stitch the chunk - auto [status, stitched] = globalStitch(chunk_images, mode, max_dim); + // Stitch the chunk using the (now ignored) preprocess flag + auto [status, stitched] = globalStitch(chunk_images, mode, max_dim, preprocess); // Build an output filename for this chunk std::string chunk_filename = run_subdir + "/chunk_" + std::to_string(chunk_counter) + "_" + @@ -106,14 +115,11 @@ void Mapping::processChunk(const std::vector& chunk_images, const std:: if (status == cv::Stitcher::OK && !stitched.empty()) { if (cv::imwrite(chunk_filename, stitched)) { - std::cout << "Saved chunk result to: " << chunk_filename << "\n"; + LOG_S(INFO) << "Saved chunk result to: " << chunk_filename << "\n"; } else { - std::cerr << "Failed to save chunk result to disk. " - << "This chunk will be lost.\n"; + LOG_F(ERROR, "Failed to save chunk result to disk. This chunk will be lost.\n"); } } else { - // If stitching fails, optionally fallback to saving each original chunk image - // or skip them. Up to you: std::cerr << "Chunk stitching failed, status = " << static_cast(status) << ". Not saving chunk.\n"; } @@ -123,7 +129,20 @@ void Mapping::processChunk(const std::vector& chunk_images, const std:: // firstPass() // ----------------------------------------------------------------------------- void Mapping::firstPass(const std::string& input_path, const std::string& run_subdir, - int chunk_size, int overlap, cv::Stitcher::Mode mode, int max_dim) { + int chunk_size, int overlap, cv::Stitcher::Mode mode, int max_dim, + bool preprocess) { + // Create a run folder if not already done + if (current_run_folder.empty()) { + current_run_folder = run_subdir + "/" + currentDateTimeStr(); + try { + fs::create_directories(current_run_folder); + LOG_S(INFO) << "Created run folder: " << current_run_folder << "\n"; + } catch (const fs::filesystem_error& e) { + LOG_S(ERROR) << "Error creating run folder: " << e.what() << "\n"; + throw; + } + } + // 1. Re-scan directory for image filenames std::vector all_filenames; for (const auto& entry : fs::directory_iterator(input_path)) { @@ -135,71 +154,66 @@ void Mapping::firstPass(const std::string& input_path, const std::string& run_su } } - // 2. Sort them so that we process in a consistent order + // 2. Sort them for consistent processing std::sort(all_filenames.begin(), all_filenames.end()); - // 3. If we have processed some images before, skip them - // i.e., only handle [processed_image_count ... end] + // 3. Skip already processed images if (static_cast(all_filenames.size()) <= processed_image_count) { - std::cout << "No new images found to process.\n"; + LOG_F(INFO, "No new images found to process.\n"); return; } std::vector new_files(all_filenames.begin() + processed_image_count, all_filenames.end()); + image_filenames = all_filenames; - // Update the total image_filenames if needed - image_filenames = all_filenames; // or keep them updated incrementally - - // 4. Load the new images into memory + // 4. Load and preprocess new images images.clear(); + Preprocess preprocessor; // instantiate the Preprocess object for (const auto& filename : new_files) { cv::Mat img = cv::imread(filename); if (!img.empty()) { + if (preprocess) { + // Apply custom preprocessing before further processing + img = preprocessor.cropRight(img); + img = readAndResize(img, max_dim); + } images.push_back(img); } else { - std::cerr << "Warning: Failed to load image: " << filename << "\n"; + LOG_S(WARNING) << "Warning: Failed to load image: " << filename << "\n"; } } + LOG_S(INFO) << "Loaded and preprocessed " << images.size() << " new images.\n"; - std::cout << "Loaded " << images.size() << " new images.\n"; - - // 5. Chunk the new images (which are currently in 'images') + // 5. Chunk the preprocessed images auto chunks = chunkListWithOverlap(static_cast(images.size()), chunk_size, overlap); - for (auto& chunk_indices : chunks) { - // Gather the images for this chunk std::vector chunk_imgs; chunk_imgs.reserve(chunk_indices.size()); for (int idx : chunk_indices) { chunk_imgs.push_back(images[idx]); } - // Process (stitch + save) - processChunk(chunk_imgs, run_subdir, mode, max_dim); + processChunk(chunk_imgs, current_run_folder, mode, max_dim, preprocess); } - // 6. Update how many images we've processed in total + // 6. Update processed count and free memory processed_image_count = static_cast(all_filenames.size()); - - // 7. Clear the images from memory after done images.clear(); } // ----------------------------------------------------------------------------- // secondPass() // ----------------------------------------------------------------------------- -void Mapping::secondPass(const std::string& run_subdir, cv::Stitcher::Mode mode, int max_dim) { - // We want to read the chunk images we saved in 'run_subdir' - // Identify all chunk files: chunk_*.jpg, chunk_*.png, etc. +void Mapping::secondPass(const std::string& run_subdir, cv::Stitcher::Mode mode, int max_dim, + bool preprocess) { + std::string folder_to_scan = current_run_folder.empty() ? run_subdir : current_run_folder; + + // Identify all chunk files (e.g., chunk_*.jpg) std::vector chunk_files; - for (const auto& entry : fs::directory_iterator(run_subdir)) { + for (const auto& entry : fs::directory_iterator(folder_to_scan)) { if (!entry.is_regular_file()) continue; - std::string filename = entry.path().filename().string(); - // Basic check: does it start with "chunk_" and end with ".jpg"? - // Or do a better pattern check if needed. if (filename.rfind("chunk_", 0) == 0) { - // starts with "chunk_" auto ext = entry.path().extension().string(); std::transform(ext.begin(), ext.end(), ext.begin(), ::tolower); if (ext == ".jpg" || ext == ".jpeg" || ext == ".png") { @@ -209,11 +223,12 @@ void Mapping::secondPass(const std::string& run_subdir, cv::Stitcher::Mode mode, } if (chunk_files.empty()) { - std::cerr << "No chunk images found in " << run_subdir << ". Nothing to stitch.\n"; + LOG_S(WARNING) << "No chunk images found in " << folder_to_scan + << ". Nothing to stitch.\n"; return; } - // Load these chunk images (should be fewer/larger than original set) + // Load chunk images (assumed to be already preprocessed) std::vector chunk_images; chunk_images.reserve(chunk_files.size()); for (const auto& cfile : chunk_files) { @@ -221,31 +236,116 @@ void Mapping::secondPass(const std::string& run_subdir, cv::Stitcher::Mode mode, if (!cimg.empty()) { chunk_images.push_back(cimg); } else { - std::cerr << "Warning: could not load chunk image: " << cfile << "\n"; + LOG_S(WARNING) << "Warning: could not load chunk image: " << cfile << "\n"; } } - std::cout << "Loaded " << chunk_images.size() << " chunk images. Attempting final stitch...\n"; + LOG_S(INFO) << "Loaded " << chunk_images.size() << " chunk images from " << folder_to_scan + << ". Attempting final stitch...\n"; - // Stitch them all to produce final - auto [status, final_stitched] = globalStitch(chunk_images, mode, max_dim); + // Stitch without additional preprocessing (images are already preprocessed) + auto [status, final_stitched] = globalStitch(chunk_images, mode, max_dim, preprocess); if (status == cv::Stitcher::OK && !final_stitched.empty()) { - // Build final filename - std::string final_name = run_subdir + "/final_" + currentDateTimeStr() + ".jpg"; + std::string final_name = folder_to_scan + "/final_" + currentDateTimeStr() + ".jpg"; if (cv::imwrite(final_name, final_stitched)) { - std::cout << "Final image saved to: " << final_name << "\n"; + LOG_S(INFO) << "Final image saved to: " << final_name << "\n"; } else { - std::cerr << "Failed to save final image to " << final_name << "\n"; + LOG_S(WARNING) << "Failed to save final image to " << final_name << "\n"; } } else { - std::cerr << "Final stitching failed. Status = " << static_cast(status) << "\n"; + LOG_S(WARNING) << "Final stitching failed. Status = " << static_cast(status) << "\n"; } - // Optionally clear chunk_images from memory chunk_images.clear(); } +// ----------------------------------------------------------------------------- +// directStitch() +// ----------------------------------------------------------------------------- +void Mapping::directStitch(const std::string& input_path, const std::string& output_path, + cv::Stitcher::Mode mode, int max_dim, bool preprocess) { + // 1. Scan directory for image filenames + std::vector all_filenames; + for (const auto& entry : fs::directory_iterator(input_path)) { + if (!entry.is_regular_file()) continue; + auto ext = entry.path().extension().string(); + std::transform(ext.begin(), ext.end(), ext.begin(), ::tolower); + if (ext == ".jpg" || ext == ".jpeg" || ext == ".png") { + all_filenames.push_back(entry.path().string()); + } + } + + // 2. Sort them for consistent processing (assuming image filenames are timestamped) + // Mapping works better when the images are sequential + std::sort(all_filenames.begin(), all_filenames.end()); + + if (all_filenames.empty()) { + LOG_S(WARNING) << "No images found in " << input_path << ". Nothing to stitch.\n"; + return; + } + + LOG_S(INFO) << "Found " << all_filenames.size() << " images to stitch.\n"; + + // 3. Load and preprocess all images + std::vector all_images; + all_images.reserve(all_filenames.size()); + Preprocess preprocessor; + + for (const auto& filename : all_filenames) { + cv::Mat img = cv::imread(filename); + if (!img.empty()) { + if (preprocess) { + // Apply custom preprocessing before further processing + // Removes the 20px green bar from Daniel's camera processing + img = preprocessor.cropRight(img); + } + img = readAndResize(img, max_dim); + all_images.push_back(img); + } else { + LOG_S(WARNING) << "Warning: Failed to load image: " << filename << "\n"; + } + } + + if (all_images.empty()) { + LOG_S(WARNING) << "No valid images loaded. Cannot stitch.\n"; + return; + } + + LOG_S(INFO) << "Loaded " << all_images.size() << " images. Starting stitching...\n"; + + // 4. Stitch all images at once + auto [status, stitched] = + globalStitch(all_images, mode, max_dim, false); // preprocess=false since already done + + // 5. Save result + if (status == cv::Stitcher::OK && !stitched.empty()) { + // Create output directory if it doesn't exist + fs::path output_file_path(output_path); + fs::path output_dir = output_file_path.parent_path(); + if (!output_dir.empty() && !fs::exists(output_dir)) { + fs::create_directories(output_dir); + } + + // If output_path is a directory, create a filename + std::string final_output_path = output_path; + if (fs::is_directory(output_path) || output_path.back() == '/') { + final_output_path = output_path + "/direct_stitch_" + currentDateTimeStr() + ".jpg"; + } + + if (cv::imwrite(final_output_path, stitched)) { + LOG_S(INFO) << "Direct stitch result saved to: " << final_output_path << "\n"; + } else { + LOG_S(WARNING) << "Failed to save stitched image to " << final_output_path << "\n"; + } + } else { + LOG_S(ERROR) << "Direct stitching failed. Status = " << static_cast(status) << "\n"; + } + + // 6. Free memory + all_images.clear(); +} + // ----------------------------------------------------------------------------- // reset() // ----------------------------------------------------------------------------- @@ -254,5 +354,6 @@ void Mapping::reset() { image_filenames.clear(); processed_image_count = 0; chunk_counter = 0; - std::cout << "Mapping state has been reset.\n"; + current_run_folder.clear(); + LOG_F(INFO, "Mapping state has been reset.\n"); } diff --git a/src/cv/matching.cpp b/src/cv/matching.cpp deleted file mode 100644 index 795b49ff..00000000 --- a/src/cv/matching.cpp +++ /dev/null @@ -1,106 +0,0 @@ -#include "cv/matching.hpp" - -#include - -/* -* IMPORTANT NOTE: -* Using torch::from_blob to convert cv::Mat to torch::Tensor results in NaN tensors -* for certain images. The solution of using std::memcpy instead was found and corroborated -* in this post: -* https://discuss.pytorch.org/t/convert-torch-tensor-to-cv-mat/42751/4 -* The old broken line is given here for reference (formerly at line 33): -* torch::Tensor tensor_image = torch::from_blob(img_dst.data, {1, 3, img_dst.rows, img_dst.cols}, torch::kF32); - -* The reason for from_blob's misbehaving tendencies may have to do with it taking in -* a pointer as its first argument (img_dst.data) as shown here: -* https://pytorch.org/cppdocs/api/function_namespacetorch_1ad7fb2a7759ef8c9443b489ddde494787.html - -* Every time we exit toInput(), img_dst is deallocated and the pointer to the image's data -* is thus rendered invalid. This results in tensors filled with NaN. -* This hypothesis is corroborated by the following post: -* https://discuss.pytorch.org/t/std-vector-torch-tensor-turns-into-nans/81853 -* If we wanted to use from_blob, we would have to perhaps pass in a constant reference to -* img, and only use img (rather than creating a new img_dst to do operations on). -* Std::memcpy works for the time being, so it will remain. -* -* Reference for resizing cv image: -* https://stackoverflow.com/questions/17533101/resize-a-matrix-after-created-it-in-opencv -*/ -auto toInput(cv::Mat img, bool show_output = false, bool unsqueeze = false, int unsqueeze_dim = 0) { - cv::Mat img_dst; - // scale the image and turn into Tensor, then input vector - cv::resize(img, img_dst, cv::Size(128, 128), 0, 0, cv::INTER_AREA); - img_dst.convertTo(img_dst, CV_32FC3); - torch::Tensor tensor_image = torch::ones({1, 3, img_dst.rows, img_dst.cols}, torch::kF32); - std::memcpy(tensor_image.data_ptr(), img_dst.data, sizeof(float)*tensor_image.numel()); - // convert back to CV image and display to verify fidelity of image if desired - if (unsqueeze) - tensor_image.unsqueeze_(unsqueeze_dim); - - if (show_output) { - std::ostringstream stream; - stream << tensor_image.slice(2, 0, 1); - LOG_F(INFO, stream.str().c_str()); - } - - return std::vector{tensor_image}; -} - -Matching::Matching(std::array - competitionObjectives, - std::vector> referenceImages, - const std::string &modelPath) - : competitionObjectives(competitionObjectives) { - - try { - this->torch_module = torch::jit::load(modelPath); - } - catch (const c10::Error& e) { - LOG_F(ERROR, "could not load the model, check if model file is present at %s. If the file is present, try to verify that it's contents are valid model weights. Sometimes when pulling from google drive, an error html page will download with the same filename if the model fails to download. \n", modelPath.c_str()); // NOLINT - throw; - } - - if (referenceImages.size() == 0) { - LOG_F(WARNING, "Empty reference image vector passed as argument\n"); - } - - // Populate referenceFeatures by putting each refImg through model - for (int i = 0; i < referenceImages.size(); i++) { - cv::Mat image = referenceImages.at(i).first; - BottleDropIndex bottle = referenceImages.at(i).second; - std::vector input = toInput(image); - torch::Tensor output = this->torch_module.forward(input).toTensor(); - this->referenceFeatures.push_back(std::make_pair(output, bottle)); - } -} - -/* -* Matches the given image with one of the referenceFeature elements -* (each referenceFeature element corresponds to an element of referenceImages, see constructor). -* @param croppedTarget - wrapper for cv::Mat image which we extract and use -* @return MatchResult - struct of bottleIndex, boolean isMatch, and minimum distance found. -* -* NOTE: Matching only occurs if loading model and ref. images was successful. -*/ -MatchResult Matching::match(const CroppedTarget& croppedTarget) { - if (referenceFeatures.empty()) { - return MatchResult(BottleDropIndex::A, -std::numeric_limits::infinity()); - } - std::vector input = toInput(croppedTarget.croppedImage); - torch::Tensor output = this->torch_module.forward(input).toTensor(); - int minIndex = 0; - double minDist = torch::pairwise_distance(output, - referenceFeatures.at(minIndex).first).item().to(); - - for (int i = 1; i < referenceFeatures.size(); i++) { - torch::Tensor curr = referenceFeatures.at(i).first; - torch::Tensor dist = torch::pairwise_distance(output, curr); - double tmp = dist.item().to(); - if (tmp < minDist) { - minDist = tmp; - minIndex = i; - } - } - BottleDropIndex bottleIdx = referenceFeatures.at(minIndex).second; - return MatchResult(bottleIdx, minDist); -} diff --git a/src/cv/pipeline.cpp b/src/cv/pipeline.cpp index f52fc423..adac69e2 100644 --- a/src/cv/pipeline.cpp +++ b/src/cv/pipeline.cpp @@ -1,72 +1,126 @@ #include "cv/pipeline.hpp" -#include "utilities/logging.hpp" + +#include + #include "protos/obc.pb.h" +#include "utilities/logging.hpp" -// TODO: eventually we will need to invoke non-default constructors for all the -// modules of the pipeline (to customize model filepath, etc...) -// TODO: I also want to have a way to customize if the model will use -// matching vs segmentation/classification -Pipeline::Pipeline(const PipelineParams& p) : - // assumes reference images passed to pipeline from not_stolen - matcher(p.competitionObjectives, p.referenceImages, p.matchingModelPath), - segmentor(p.segmentationModelPath), - detector(p.saliencyModelPath) {} - -/* - * Entrypoint of CV Pipeline. At a high level, it will include the following - * stages: - * - Saliency takes the full size image and outputs any cropped targets - * that are detected - * - Cropped targets are passed into target matching where they will - * be compared against targets associated with each water bottle. - * All targets (regardless of match status) will be passed onto - * later stages. - * - Bounding boxes predicted from saliency and GPS telemetry from when - * the photo was captured are passed into the localization algorithm. - * Here, the real latitude/longitude coordinates of the target are - * calculated. - * - Note, we may use a slight variation of this pipeline where the - * target matching stage is replaced by segmentation/classification. - */ -PipelineResults Pipeline::run(const ImageData &imageData) { +// Pipeline constructor: initialize YOLO detector and the preprocess flag. +Pipeline::Pipeline(const PipelineParams& p) + : yoloDetector(std::make_unique(p.yoloModelPath, 0.05f, 640, 640)), + outputPath(p.outputPath), + do_preprocess(p.do_preprocess) {} + +PipelineResults Pipeline::run(const ImageData& imageData) { LOG_F(INFO, "Running pipeline on an image"); - VLOG_F(TRACE, "Saliency Start"); - std::vector saliencyResults = this->detector.salience(imageData.DATA); + // Preprocess the image if enabled; otherwise, use the original. + cv::Mat processedImage = imageData.DATA; + if (do_preprocess) { + processedImage = preprocessor.cropRight(imageData.DATA); + } + + // 1) YOLO DETECTION using the (possibly preprocessed) image + std::vector yoloResults = this->yoloDetector->detect(processedImage); - // if saliency finds no potential targets, end early with no results - if (saliencyResults.empty()) { - LOG_F(INFO, "No saliency results, terminating..."); - return PipelineResults(imageData, {}); + // If YOLO finds no potential targets, we can return early (still saving out the final image). + if (yoloResults.empty()) { + LOG_F(INFO, "No YOLO detections, terminating..."); + + if (!outputPath.empty()) { + static std::atomic file_counter{0}; + int i = file_counter.fetch_add(1); + + // Build a unique filename + std::string uniqueOutputPath; + size_t dotPos = outputPath.find_last_of('.'); + size_t sepPos = outputPath.find_last_of("/\\"); + if (dotPos != std::string::npos && (sepPos == std::string::npos || dotPos > sepPos)) { + uniqueOutputPath = outputPath.substr(0, dotPos) + "_" + std::to_string(i) + + outputPath.substr(dotPos); + } else { + uniqueOutputPath = outputPath + "_" + std::to_string(i) + ".jpg"; + } + + if (!cv::imwrite(uniqueOutputPath, processedImage)) { + LOG_F(ERROR, "Failed to write image to %s", uniqueOutputPath.c_str()); + } else { + LOG_F(INFO, "Output image saved to %s", uniqueOutputPath.c_str()); + } + } + + // Return the processed image (with no detections) + ImageData processedData = imageData; + processedData.DATA = processedImage; + return PipelineResults(processedData, {}); } - VLOG_F(TRACE, "Saliency cropped %ld potential targets", saliencyResults.size()); + // 2) BUILD DETECTED TARGETS & LOCALIZE std::vector detectedTargets; - size_t curr_target_num = 0; - for (CroppedTarget target : saliencyResults) { - VLOG_F(TRACE, "Working on target %ld/%ld", ++curr_target_num, saliencyResults.size()); - VLOG_F(TRACE, "Matching Start"); - MatchResult potentialMatch = this->matcher.match(target); - - VLOG_F(TRACE, "Localization Start"); - // TODO: determine what to do if image is missing telemetry metadata + detectedTargets.reserve(yoloResults.size()); + + for (const auto& det : yoloResults) { + // Fill bounding box + Bbox box; + box.x1 = static_cast(det.x1); + box.y1 = static_cast(det.y1); + box.x2 = static_cast(det.x2); + box.y2 = static_cast(det.y2); + + // If you want to keep cropping code, you can call: + // cv::Mat cropped = crop(processedImage, box); + + // Localize GPSCoord targetPosition; if (imageData.TELEMETRY.has_value()) { - // targetPosition = GPSCoord(this->ecefLocalizer.localize( - // imageData.TELEMETRY.value(), target.bbox)); - targetPosition.set_latitude(imageData.TELEMETRY.value().latitude_deg); - targetPosition.set_longitude(imageData.TELEMETRY.value().longitude_deg); - targetPosition.set_altitude(0); + targetPosition = this->gsdLocalizer.localize(imageData.TELEMETRY.value(), box); } - VLOG_F(TRACE, "Detected target %ld/%ld at [%f, %f] matched to bottle %d with %f distance.", - curr_target_num, saliencyResults.size(), - targetPosition.latitude(), targetPosition.longitude(), - static_cast(potentialMatch.bottleDropIndex), potentialMatch.distance); - detectedTargets.push_back(DetectedTarget(targetPosition, - potentialMatch.bottleDropIndex, potentialMatch.distance, target)); + + // Populate your DetectedTarget + DetectedTarget detected; + detected.bbox = box; + detected.coord = targetPosition; + detected.likely_airdrop = static_cast(det.class_id); + detected.match_distance = (det.confidence > 0.f) ? (1.0 / det.confidence) : 9999.0; + + detectedTargets.push_back(detected); + } + + // 3) DRAW DETECTIONS ON THE IMAGE + // (this modifies processedImage in-place) + this->yoloDetector->drawAndPrintDetections(processedImage, yoloResults); + + // Save the annotated image if an output path is specified + if (!outputPath.empty()) { + static std::atomic file_counter{0}; + int i = file_counter.fetch_add(1); + std::string uniqueOutputPath; + + size_t dotPos = outputPath.find_last_of('.'); + size_t sepPos = outputPath.find_last_of("/\\"); + if (dotPos != std::string::npos && (sepPos == std::string::npos || dotPos > sepPos)) { + uniqueOutputPath = + outputPath.substr(0, dotPos) + "_" + std::to_string(i) + outputPath.substr(dotPos); + } else { + uniqueOutputPath = outputPath + "_" + std::to_string(i) + ".jpg"; + } + + if (!cv::imwrite(uniqueOutputPath, processedImage)) { + LOG_F(ERROR, "Failed to write image to %s", uniqueOutputPath.c_str()); + } else { + LOG_F(INFO, "Output image saved to %s", uniqueOutputPath.c_str()); + } } LOG_F(INFO, "Finished Pipeline on an image"); - return PipelineResults(imageData, detectedTargets); + + // Wrap up the final annotated image to return + ImageData processedData = imageData; + processedData.DATA = processedImage; + + // Return a PipelineResults that includes: + // 1) The final big image with bounding boxes drawn + // 2) The bounding boxes + localized GPS coords + return PipelineResults(processedData, detectedTargets); } diff --git a/src/cv/preprocess.cpp b/src/cv/preprocess.cpp new file mode 100644 index 00000000..926e1e14 --- /dev/null +++ b/src/cv/preprocess.cpp @@ -0,0 +1,20 @@ +#include "cv/preprocess.hpp" + +#include + +cv::Mat Preprocess::cropRight(const cv::Mat &image) const { + // Check if the image is wide enough for cropping. + if (image.cols <= crop_pixels_) { + // Optionally, you might want to handle this case differently, + // such as throwing an error. Here, we return a clone of the input. + return image.clone(); + } + + // Define the region-of-interest: start at (0,0), include full height, + // and reduce the width by crop_pixels_. + cv::Rect roi(0, 0, image.cols - crop_pixels_, image.rows); + + // Crop the image and return a new copy. + cv::Mat croppedImage = image(roi).clone(); + return croppedImage; +} diff --git a/src/cv/saliency.cpp b/src/cv/saliency.cpp deleted file mode 100644 index 3d6f9c32..00000000 --- a/src/cv/saliency.cpp +++ /dev/null @@ -1,155 +0,0 @@ -#include "cv/saliency.hpp" -#include // One-stop header. -#include -#include -#include -#include -#include -#include "utilities/logging.hpp" - - -#define REGULAR_TARGET 1 -#define MANNIKIN 2 - -// constructor definition -Saliency::Saliency(std::string modelPath) { - this->modelPath = modelPath; - - // use cuda if it is available - c10::Device device = torch::cuda::is_available() ? torch::kCUDA : torch::kCPU; - torch::jit::script::Module module; - try { - // Deserialize the ScriptModule from a file using torch::jit::load(). - this->module = torch::jit::load(modelPath, device); - this->module.eval(); - } - catch (const c10::Error& e) { - LOG_F(ERROR, "error loading the model %s", e.msg().c_str()); - } -} - -std::vector Saliency::salience(cv::Mat image) { - at::Tensor tensor = Saliency::ToTensor(image, false, false, 0); - // preparing our tensor - // convert the tensor into float and scale it - tensor = tensor.toType(c10::kFloat).div(255); - // swap axis - tensor = Saliency::transpose(tensor, { (2), (0), (1) }); - - // eventually add device as member of Saliency - c10::Device device = torch::cuda::is_available() ? torch::kCUDA : torch::kCPU; - auto tensor_cuda = tensor.to(device); - - auto input_to_net = ToInput(tensor_cuda); - - /* - * forward() runs an inference on the input image using the provided model - * and returns predictions as List[Dict[Tensor]], where the fields of Dict - * that we want are : a) boxes (FloatTensor[N, 4]): the predicted boxes, and - * b) scores (Tensor[N]): the scores of each detection. - */ - - // output is a tuple of (losses, detections) - auto output = module.forward(input_to_net); - c10::ivalue::Tuple& tuple = output.toTupleRef(); - // detections is our List[Dict[Tensor]], and index 1 contains the detections in the image - auto detections = tuple.elements()[1]; - // since we only passed in one image, listDetections will only have one element - auto listDetections = detections.toList(); - - std::vector targets; - targets = extractTargets(listDetections, image); - - LOG_F(INFO, "saliency found %ld targets", targets.size()); - for (auto const& target : targets) { - LOG_F(INFO, "\ttarget at bbox: (%d, %d, %d, %d). ismannikin: %d", - target.bbox.x1, - target.bbox.y1, - target.bbox.x2, - target.bbox.y2, - target.isMannikin); - } - return targets; -} - -// show_output flag can be used to print tensor_image.slice(2, 0, 1) -at::Tensor Saliency::ToTensor(cv::Mat img, bool show_output = false, bool unsqueeze = false, - int unsqueeze_dim = 0) { - at::Tensor tensor_image = torch::from_blob(img.data, { img.rows, img.cols, 3 }, at::kByte); - - if (unsqueeze) { - tensor_image.unsqueeze_(unsqueeze_dim); - } - - return tensor_image; -} - -at::Tensor Saliency::transpose(at::Tensor tensor, c10::IntArrayRef dims = { 0, 3, 1, 2 }) { - tensor = tensor.permute(dims); - return tensor; -} - -std::vector Saliency::ToInput(at::Tensor tensor_image) { - // Create a vector of inputs. - std::vector toReturn; // c10 List is also IValue - c10::List tensorList; - tensorList.push_back(tensor_image); // add Tensor to c10::List - toReturn.push_back(tensorList); // add c10::List to vector of IValues - return toReturn; -} - -std::vector Saliency::extractTargets(c10::List listDetections, - cv::Mat image) { - std::vector targetList; - - // looping through the number of images (we only have one image) - for (c10::IValue results : listDetections) { - /* - * Each c10::IValue in results is a dict whose fields are boxes, labels, scores. - * we only need to extract boxes (bounding boxes) and labels (mannikin or not?). - */ - c10::Dict dict = results.toGenericDict(); - c10::IValue boxes = dict.at("boxes"); - c10::IValue labels = dict.at("labels"); - at::Tensor boxTensor = boxes.toTensor(); - at::Tensor labelsTensor = labels.toTensor(); - - // looping through the number of bounding boxes in the image identified by the model - for (int i = 0; i < boxTensor.size(0); i++) { - /* - * format of box: [x1, y1, x2, y2], with 0 <= x1 < x2 <= W and 0 <= y1 < y2 <= H - */ - CroppedTarget target; - Bbox box; - box.x1 = boxTensor[i][0].item().toInt(); - box.y1 = boxTensor[i][1].item().toInt(); - box.x2 = boxTensor[i][2].item().toInt(); - box.y2 = boxTensor[i][3].item().toInt(); - /* - * label interpretation: - * 1 if regular target - * 2 if mannikin - */ - if (labelsTensor[0].item().toInt() == MANNIKIN) { - target.isMannikin = 1; - } else if (labelsTensor[0].item().toInt() == REGULAR_TARGET) { - target.isMannikin = 0; - } else { - LOG_F(ERROR, "Saliency::salience found invalid score"); - } - - cv::Rect roi; // setup region of interest (cropped target) - roi.x = box.x1; - roi.y = box.y1; - roi.width = box.x2 - box.x1; - roi.height = box.y2 - box.y1; - cv::Mat cropdImg = image(roi); - - target.bbox = box; - target.croppedImage = cropdImg; - - targetList.push_back(target); - } - } - return targetList; -} diff --git a/src/cv/segmentation.cpp b/src/cv/segmentation.cpp deleted file mode 100644 index 7cd5e86c..00000000 --- a/src/cv/segmentation.cpp +++ /dev/null @@ -1,114 +0,0 @@ -#include "cv/segmentation.hpp" - -#include - -Segmentation::Segmentation(const std::string &modelPath) { - try { - this->module = torch::jit::load(modelPath); - } - catch (const c10::Error& e) { - LOG_F(ERROR, "could not load the model, check if model file is present at %s. If the file is present, try to verify that it's contents are valid model weights. Sometimes when pulling from google drive, an error html page will download with the same filename if the model fails to download. \n", modelPath.c_str()); // NOLINT - throw; - } - } - -SegmentationResults Segmentation::segment(const CroppedTarget &target) { - auto device = torch::kCPU; - auto fcn = this->module; - fcn.to(device); - - auto target_tensor = ToTensor(target.croppedImage, false, false, 0); - target_tensor = target_tensor.clamp_max(c10::Scalar(50)); - target_tensor = target_tensor.toType(c10::kFloat).div(255); - target_tensor = transpose(target_tensor, { (2), (0), (1) }); - target_tensor.unsqueeze_(0); - - auto input_to_net = ToInput(target_tensor); - at::Tensor output = (fcn.forward(input_to_net).toGenericDict().begin()->value()).toTensor(); - torch::Tensor cpu_tensor = output.to(device); - cpu_tensor = cpu_tensor[0]; - cpu_tensor = torch::softmax(cpu_tensor, 0); - - auto character_mask = cpu_tensor[1]; - auto shape_mask = cpu_tensor[0]; - cv::Mat character_mask_mat(character_mask.sizes()[0], character_mask.sizes()[1], - CV_32FC1, character_mask.data_ptr()); - cv::Mat shape_mask_mat(shape_mask.sizes()[0], shape_mask.sizes()[1], - CV_32FC1, shape_mask.data_ptr()); - - SegmentationResults result; - result.shapeMask = character_mask_mat; - result.characterMask = shape_mask_mat; - return result; -} - - - -std::string get_image_type(const cv::Mat& img, bool more_info = true) { - std::string r; - int type = img.type(); - uchar depth = type & CV_MAT_DEPTH_MASK; - uchar chans = 1 + (type >> CV_CN_SHIFT); - - switch (depth) { - case CV_8U: r = "8U"; break; - case CV_8S: r = "8S"; break; - case CV_16U: r = "16U"; break; - case CV_16S: r = "16S"; break; - case CV_32S: r = "32S"; break; - case CV_32F: r = "32F"; break; - case CV_64F: r = "64F"; break; - default: r = "User"; break; - } - - r += "C"; - r += (chans + '0'); - - if (more_info) { - LOG_F(INFO, "depth: %d channels: %d\n", img.depth(), img.channels()); - } - return r; -} - -void show_image(cv::Mat& img, std::string title) { - std::string image_type = get_image_type(img); - cv::namedWindow(title + " type:" + image_type, cv::WINDOW_NORMAL); - cv::imshow(title, img); - cv::waitKey(0); -} - -at::Tensor transpose(at::Tensor tensor, c10::IntArrayRef dims = { 0, 3, 1, 2 }) { - tensor = tensor.permute(dims); - return tensor; -} - -at::Tensor ToTensor(cv::Mat img, bool show_output = false, bool unsqueeze = false, -int unsqueeze_dim = 0) { - at::Tensor tensor_image = torch::from_blob(img.data, { img.rows, img.cols, 3 }, at::kByte); - - if (unsqueeze) { - tensor_image.unsqueeze_(unsqueeze_dim); - } - - return tensor_image; -} - -std::vector ToInput(at::Tensor tensor_image) { - // Create a vector of inputs. - return std::vector{tensor_image}; -} - -cv::Mat ToCvImage(at::Tensor tensor) { - int width = tensor.sizes()[0]; - int height = tensor.sizes()[1]; - try { - cv::Mat output_mat(cv::Size{ height, width }, CV_8UC3, tensor.data_ptr()); - - show_image(output_mat, "converted image from tensor"); - return output_mat.clone(); - } - catch (const c10::Error& e) { - LOG_S(ERROR) << "error loading the model: : " << e.msg() << "\n"; - } - return cv::Mat(height, width, CV_8UC3); -} diff --git a/src/cv/yolo.cpp b/src/cv/yolo.cpp new file mode 100644 index 00000000..e17e6436 --- /dev/null +++ b/src/cv/yolo.cpp @@ -0,0 +1,246 @@ +#include "cv/yolo.hpp" + +#include + +// For simplicity, we are not doing advanced error handling +// Make sure to catch and handle exceptions in production code + +YOLO::YOLO(const std::string& modelPath, float confThreshold, int inputWidth, int inputHeight) + : env_(ORT_LOGGING_LEVEL_WARNING, "yolo-inference"), + session_(nullptr), + confThreshold_(confThreshold), + inputWidth_(inputWidth), + inputHeight_(inputHeight) { + // Initialize session options if needed + sessionOptions_.SetGraphOptimizationLevel(GraphOptimizationLevel::ORT_ENABLE_EXTENDED); + + // Create the session + try { + session_ = new Ort::Session(env_, modelPath.c_str(), sessionOptions_); + std::cout << "Model loaded successfully.\n"; + } catch (const Ort::Exception& e) { + std::cerr << "Error loading model: " << e.what() << std::endl; + } + + // Create an allocator + Ort::AllocatorWithDefaultOptions allocator; + + // Get input names + size_t numInputNodes = session_->GetInputCount(); + inputNames_.resize(numInputNodes); + for (size_t i = 0; i < numInputNodes; i++) { + auto name = session_->GetInputNameAllocated(i, allocator); + inputNames_[i] = std::string(name.get()); + } + + // Get output names + size_t numOutputNodes = session_->GetOutputCount(); + outputNames_.resize(numOutputNodes); + for (size_t i = 0; i < numOutputNodes; i++) { + auto name = session_->GetOutputNameAllocated(i, allocator); // function for output nodes + outputNames_[i] = std::string(name.get()); + } +} + +YOLO::~YOLO() { + if (session_) { + delete session_; + session_ = nullptr; + } +} + +std::vector YOLO::preprocess(const cv::Mat& image) { + // Compute letterbox scale + float r = std::min(static_cast(inputWidth_) / static_cast(image.cols), + static_cast(inputHeight_) / static_cast(image.rows)); + + int unpadW = std::round(image.cols * r); + int unpadH = std::round(image.rows * r); + int dw = inputWidth_ - unpadW; // total horizontal padding + int dh = inputHeight_ - unpadH; // total vertical padding + + // We'll store these so we can "un-letterbox" later: + scale_ = r; + padLeft_ = dw / 2; // per-side left padding + padTop_ = dh / 2; // per-side top padding + + // Then call letterbox(...). This returns a new image of size (inputWidth_ x inputHeight_), + // but we know how it was scaled & padded: + cv::Mat resized = letterbox(image, inputWidth_, inputHeight_, cv::Scalar(114, 114, 114)); + + // Convert BGR->RGB if your model is trained on RGB: + cv::cvtColor(resized, resized, cv::COLOR_BGR2RGB); + + // Convert to float [0..1] + resized.convertTo(resized, CV_32F, 1.0f / 255.0f); + + // HWC -> CHW + std::vector inputData(inputWidth_ * inputHeight_ * 3); + size_t index = 0; + for (int c = 0; c < 3; c++) { + for (int h = 0; h < inputHeight_; h++) { + for (int w = 0; w < inputWidth_; w++) { + inputData[index++] = resized.at(h, w)[c]; + } + } + } + + return inputData; +} + +std::vector YOLO::detect(const cv::Mat& image) { + // Preprocess the image + std::vector inputTensorValues = preprocess(image); + + // Create input tensor object from data values + std::vector inputShape = {1, 3, static_cast(inputHeight_), + static_cast(inputWidth_)}; + size_t inputTensorSize = 1 * 3 * inputHeight_ * inputWidth_; + Ort::MemoryInfo memoryInfo = Ort::MemoryInfo::CreateCpu(OrtArenaAllocator, OrtMemTypeDefault); + Ort::Value inputTensor = + Ort::Value::CreateTensor(memoryInfo, inputTensorValues.data(), inputTensorSize, + inputShape.data(), inputShape.size()); + + // Convert std::vector to std::vector + std::vector inputNamesCStr; + for (const auto& name : inputNames_) { + inputNamesCStr.push_back(name.c_str()); + } + + std::vector outputNamesCStr; + for (const auto& name : outputNames_) { + outputNamesCStr.push_back(name.c_str()); + } + + // Run inference using the C-style string arrays + auto outputTensors = + session_->Run(Ort::RunOptions{nullptr}, inputNamesCStr.data(), &inputTensor, 1, + outputNamesCStr.data(), outputNamesCStr.size()); + + // Extract output + float* outputData = outputTensors[0].GetTensorMutableData(); + + std::vector detections; + size_t numDetections = 300; // for example + size_t elementsPerDetection = 6; // [x1, y1, x2, y2, conf, classID] + + auto shapeInfo = outputTensors[0].GetTensorTypeAndShapeInfo(); + auto shape = shapeInfo.GetShape(); + + // Parse the output data + for (size_t i = 0; i < numDetections; i++) { + size_t offset = i * elementsPerDetection; + float x1 = outputData[offset + 0]; + float y1 = outputData[offset + 1]; + float x2 = outputData[offset + 2]; + float y2 = outputData[offset + 3]; + float conf = outputData[offset + 4]; + float cls = outputData[offset + 5]; + + if (conf >= confThreshold_) { + // Remove the letterbox offset + x1 -= padLeft_; + y1 -= padTop_; + x2 -= padLeft_; + y2 -= padTop_; + + // Scale back to original image + x1 /= scale_; + y1 /= scale_; + x2 /= scale_; + y2 /= scale_; + + // Clamp coords to image boundaries + x1 = std::max(0.f, std::min(x1, static_cast(image.cols) - 1)); + y1 = std::max(0.f, std::min(y1, static_cast(image.rows) - 1)); + x2 = std::max(0.f, std::min(x2, static_cast(image.cols) - 1)); + y2 = std::max(0.f, std::min(y2, static_cast(image.rows) - 1)); + + Detection det; + det.x1 = x1; + det.y1 = y1; + det.x2 = x2; + det.y2 = y2; + det.confidence = conf; + det.class_id = static_cast(cls); + + detections.push_back(det); + } + } + + return detections; +} + +cv::Mat YOLO::letterbox(const cv::Mat& src, int newWidth, int newHeight, const cv::Scalar& color) { + float r = std::min(static_cast(newWidth) / static_cast(src.cols), + static_cast(newHeight) / static_cast(src.rows)); + + int unpadW = std::round(src.cols * r); + int unpadH = std::round(src.rows * r); + + cv::Mat dst; + cv::resize(src, dst, cv::Size(unpadW, unpadH)); + + int dw = newWidth - unpadW; + int dh = newHeight - unpadH; + + int top = std::round(dh / 2.0f); + int bottom = dh - top; + int left = std::round(dw / 2.0f); + int right = dw - left; + + cv::copyMakeBorder(dst, dst, top, bottom, left, right, cv::BORDER_CONSTANT, color); + return dst; +} + +void YOLO::drawAndPrintDetections(cv::Mat& image, const std::vector& detections) { + // Iterate through each detection and draw + for (const auto& det : detections) { + // Print detection info + std::cout << "Detected class: " << det.class_id << " conf: " << det.confidence << " box: [" + << det.x1 << ", " << det.y1 << ", " << det.x2 << ", " << det.y2 << "]" + << std::endl; + + // Draw the bounding box + cv::rectangle(image, cv::Point(static_cast(det.x1), static_cast(det.y1)), + cv::Point(static_cast(det.x2), static_cast(det.y2)), + cv::Scalar(0, 255, 0), // green box + 2); + + // Draw label (class + confidence) above the box + std::string label = "ID:" + std::to_string(det.class_id) + + " conf:" + std::to_string(det.confidence).substr(0, 4); + cv::putText(image, label, cv::Point(static_cast(det.x1), static_cast(det.y1) - 5), + cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(0, 255, 0), 1); + } +} + +void YOLO::processAndSaveImage(const cv::Mat& image, const std::string& outputPath) { + cv::Mat outputImage = image.clone(); + std::vector detections = detect(outputImage); + drawAndPrintDetections(outputImage, detections); + + // Generate a unique filename using a static atomic counter. + static std::atomic file_counter{0}; + int counter = file_counter.fetch_add(1); + + std::string uniqueOutputPath; + // Find the position of the last dot and the last path separator + size_t dotPos = outputPath.find_last_of('.'); + size_t sepPos = outputPath.find_last_of("/\\"); // supports both UNIX and Windows paths + + // If a dot exists after the last separator, assume it's a file extension. + if (dotPos != std::string::npos && (sepPos == std::string::npos || dotPos > sepPos)) { + uniqueOutputPath = outputPath.substr(0, dotPos) + "_" + std::to_string(counter) + + outputPath.substr(dotPos); + } else { + // No extension found; default to appending counter and .jpg extension. + uniqueOutputPath = outputPath + "_" + std::to_string(counter) + ".jpg"; + } + + if (!cv::imwrite(uniqueOutputPath, outputImage)) { + std::cerr << "Failed to write output image to " << uniqueOutputPath << std::endl; + } else { + std::cout << "Output saved to " << uniqueOutputPath << std::endl; + } +} diff --git a/src/network/CMakeLists.txt b/src/network/CMakeLists.txt index 8a2d9f5a..c8831462 100644 --- a/src/network/CMakeLists.txt +++ b/src/network/CMakeLists.txt @@ -45,6 +45,7 @@ target_add_httplib(${LIB_NAME}) target_add_mavsdk(${LIB_NAME}) target_add_matplot(${LIB_NAME}) target_add_loguru(${LIB_NAME}) +target_add_onnxruntime(${LIB_NAME}) target_include_directories(${LIB_NAME} PRIVATE ${ImageMagick_INCLUDE_DIRS}) target_link_libraries(${LIB_NAME} PRIVATE -Wl,--copy-dt-needed-entries ${ImageMagick_LIBRARIES}) diff --git a/src/network/airdrop_client.cpp b/src/network/airdrop_client.cpp index edf4d734..eebda600 100644 --- a/src/network/airdrop_client.cpp +++ b/src/network/airdrop_client.cpp @@ -15,8 +15,8 @@ using namespace std::chrono_literals; // NOLINT AirdropClient::AirdropClient(ad_socket_t socket) { this->socket = socket; auto time = getUnixTime_ms(); - for (int curr_bottle = UDP2_A; curr_bottle <= UDP2_E; curr_bottle++) { - this->last_heartbeat[curr_bottle - 1] = time; + for (int curr_airdrop = UDP2_A; curr_airdrop <= UDP2_D; curr_airdrop++) { + this->last_heartbeat[curr_airdrop - 1] = time; } this->_establishConnection(); // block until connection established } @@ -48,7 +48,7 @@ void AirdropClient::_establishConnection() { [this, &stop]() { loguru::set_thread_name("airdrop reset spam"); while (true) { - LOG_F(INFO, "Sending reset packets to all bottles..."); + LOG_F(INFO, "Sending reset packets to all airdrops..."); send_ad_packet(this->socket, makeResetPacket(UDP2_ALL)); std::this_thread::sleep_for(10s); if (stop) { @@ -110,15 +110,15 @@ std::optional AirdropClient::receive() { return packet; } -std::list> +std::list> AirdropClient::getLostConnections(std::chrono::milliseconds threshold) { - std::list> list; + std::list> list; auto time = getUnixTime_ms(); for (int i = 0; i < this->last_heartbeat.size(); i++) { auto time_since_last_heartbeat = time - this->last_heartbeat[i]; if (time_since_last_heartbeat >= threshold) { - list.push_back({static_cast(i + 1), time_since_last_heartbeat}); + list.push_back({static_cast(i + 1), time_since_last_heartbeat}); } } @@ -163,9 +163,9 @@ packet_t AirdropClient::_receiveBlocking() { } // TODO: helper to go from packet -> str - uint8_t bottle, state; - parseID(packet.id, &bottle, &state); - VLOG_F(TRACE, "received airdrop packet: %hhu %hhu %hhu", packet.header, bottle, state); + uint8_t airdrop, state; + parseID(packet.id, &airdrop, &state); + VLOG_F(TRACE, "received airdrop packet: %hhu %hhu %hhu", packet.header, airdrop, state); return packet; } @@ -187,10 +187,10 @@ void AirdropClient::_receiveWorker() { } if (packet.header == SET_MODE) { - uint8_t bottle, state; - parseID(packet.id, &bottle, &state); + uint8_t airdrop, state; + parseID(packet.id, &airdrop, &state); send_ad_packet(this->socket, makeModePacket(ACK_MODE, - static_cast(bottle), OBC_NULL, *this->mode)); + static_cast(airdrop), OBC_NULL, *this->mode)); LOG_F(INFO, "Received extra SET_MODE, reacking"); continue; } @@ -207,20 +207,20 @@ bool AirdropClient::_parseHeartbeats(packet_t packet) { return false; } - uint8_t bottle, state; - parseID(packet.id, &bottle, &state); + uint8_t airdrop, state; + parseID(packet.id, &airdrop, &state); - if (bottle < UDP2_A || bottle > UDP2_E) { - LOG_F(ERROR, "ERROR: invalid bottle heartbeat (from bottle %d?)", bottle); + if (airdrop < UDP2_A || airdrop > UDP2_D) { + LOG_F(ERROR, "ERROR: invalid airdrop heartbeat (from airdrop %d?)", airdrop); return false; } // Valid heartbeat packet, so packet.data is within // [1, 5] and corresponds to a bottle index, so we can // subtract 1 to get the index into the lastHeartbeat array. - this->last_heartbeat[bottle - 1] = getUnixTime_ms(); + this->last_heartbeat[airdrop - 1] = getUnixTime_ms(); - LOG_F(INFO, "Packet heartbeat from %d", bottle); + LOG_F(INFO, "Packet heartbeat from %d", airdrop); return true; } diff --git a/src/network/gcs.cpp b/src/network/gcs.cpp index 77c4034f..64995fcb 100644 --- a/src/network/gcs.cpp +++ b/src/network/gcs.cpp @@ -2,27 +2,25 @@ #include -#include #include #include #include +#include #include #include "core/mission_parameters.hpp" #include "core/mission_state.hpp" -#include "ticks/tick.hpp" +#include "network/gcs_routes.hpp" +#include "pathing/cartesian.hpp" +#include "protos/obc.pb.h" #include "ticks/path_gen.hpp" +#include "ticks/tick.hpp" #include "utilities/locks.hpp" -#include "utilities/serialize.hpp" #include "utilities/logging.hpp" -#include "protos/obc.pb.h" -#include "pathing/cartesian.hpp" -#include "network/gcs_routes.hpp" - +#include "utilities/serialize.hpp" GCSServer::GCSServer(uint16_t port, std::shared_ptr state) - :port{port}, state{state} -{ + : port{port}, state{state} { if (port < 1024) { LOG_F(ERROR, "Ports 0-1023 are reserved. Using port %d as a fallback...", DEFAULT_GCS_PORT); port = DEFAULT_GCS_PORT; @@ -66,13 +64,15 @@ void GCSServer::_bindHandlers() { BIND_HANDLER(Post, dodropnow); BIND_HANDLER(Post, targets, locations); - BIND_HANDLER(Post, targets, reject); - BIND_HANDLER(Post, targets, validate); BIND_HANDLER(Get, targets, all); - BIND_HANDLER(Get, targets, matched); BIND_HANDLER(Post, targets, matched); BIND_HANDLER(Post, kill, kill, kill); - BIND_HANDLER(Get, oh, shit); + BIND_HANDLER(Post, camera, startstream); + BIND_HANDLER(Post, camera, endstream); + + BIND_HANDLER(Get, tickstate); + + // BIND_HANDLER(Get, oh, shit); } diff --git a/src/network/gcs_routes.cpp b/src/network/gcs_routes.cpp index f2287251..3c65469e 100644 --- a/src/network/gcs_routes.cpp +++ b/src/network/gcs_routes.cpp @@ -1,35 +1,44 @@ #include #include -#include +#include #include -#include #include -#include +#include +#include + +#include #include "core/mission_state.hpp" -#include "protos/obc.pb.h" -#include "utilities/serialize.hpp" -#include "utilities/logging.hpp" -#include "utilities/http.hpp" -#include "pathing/mission_path.hpp" #include "network/gcs_macros.hpp" -#include "ticks/tick.hpp" +#include "network/mavlink.hpp" +#include "pathing/mission_path.hpp" +#include "protos/obc.pb.h" +#include "ticks/airdrop_approach.hpp" +#include "ticks/cv_loiter.hpp" #include "ticks/path_gen.hpp" #include "ticks/path_validate.hpp" +#include "ticks/tick.hpp" #include "ticks/wait_for_takeoff.hpp" -#include "ticks/cv_loiter.hpp" +#include "utilities/http.hpp" +#include "utilities/logging.hpp" +#include "utilities/serialize.hpp" + +extern "C" { +#include "udp_squared/protocol.h" +} -using namespace std::chrono_literals; // NOLINT +using namespace std::chrono_literals; // NOLINT +using json = nlohmann::json; /* * This file defines all of the GCS handler functions for every route * the gcs server is listening on. - * + * * Inside of each of the functions, you have access to these variables - * + * * state: std::shared_ptr - * This is a shared pointer to the mission state, just like + * This is a shared pointer to the mission state, just like * other parts of the code have available. * request: const httplib::Request& * Lets you access all of the information about the HTTP request @@ -40,17 +49,15 @@ using namespace std::chrono_literals; // NOLINT * the LOG_RESPONSE macro will handle it for you. */ - DEF_GCS_HANDLE(Get, connections) { LOG_REQUEST_TRACE("GET", "/connections"); - std::list> lost_airdrop_conns; + std::list> lost_airdrop_conns; if (state->getAirdrop() == nullptr) { - lost_airdrop_conns.push_back({BottleDropIndex::A, 99999ms}); - lost_airdrop_conns.push_back({BottleDropIndex::B, 99999ms}); - lost_airdrop_conns.push_back({BottleDropIndex::C, 99999ms}); - lost_airdrop_conns.push_back({BottleDropIndex::D, 99999ms}); - lost_airdrop_conns.push_back({BottleDropIndex::E, 99999ms}); + lost_airdrop_conns.push_back({AirdropIndex::Kaz, 99999ms}); + lost_airdrop_conns.push_back({AirdropIndex::Kimi, 99999ms}); + lost_airdrop_conns.push_back({AirdropIndex::Chris, 99999ms}); + lost_airdrop_conns.push_back({AirdropIndex::Daniel, 99999ms}); } else { lost_airdrop_conns = state->getAirdrop()->getLostConnections(3s); } @@ -64,7 +71,6 @@ DEF_GCS_HANDLE(Get, connections) { mav_conn = state->getMav()->get_conn_status(); } - bool camera_good = false; if (state->getCamera()) { @@ -72,8 +78,8 @@ DEF_GCS_HANDLE(Get, connections) { } OBCConnInfo info; - for (auto const& [bottle_index, ms_since_last_heartbeat] : lost_airdrop_conns) { - info.add_dropped_bottle_idx(bottle_index); + for (auto const& [airdrop_index, ms_since_last_heartbeat] : lost_airdrop_conns) { + info.add_dropped_airdrop_idx(airdrop_index); info.add_ms_since_ad_heartbeat(ms_since_last_heartbeat.count()); } info.set_mav_rc_good(mav_conn.is_available); @@ -150,17 +156,15 @@ DEF_GCS_HANDLE(Post, targets, locations) { for (const auto& waypoint : waypoints) { google::protobuf::util::JsonStringToMessage(waypoint.dump(), &airdrop_target); - bottle_t bottle; - if (airdrop_target.index() == BottleDropIndex::A) { - bottle = UDP2_A; - } else if (airdrop_target.index() == BottleDropIndex::B) { - bottle = UDP2_B; - } else if (airdrop_target.index() == BottleDropIndex::C) { - bottle = UDP2_C; - } else if (airdrop_target.index() == BottleDropIndex::D) { - bottle = UDP2_D; - } else if (airdrop_target.index() == BottleDropIndex::E) { - bottle = UDP2_E; + airdrop_t airdrop; + if (airdrop_target.index() == AirdropIndex::Kaz) { + airdrop = UDP2_A; + } else if (airdrop_target.index() == AirdropIndex::Kimi) { + airdrop = UDP2_B; + } else if (airdrop_target.index() == AirdropIndex::Chris) { + airdrop = UDP2_C; + } else if (airdrop_target.index() == AirdropIndex::Daniel) { + airdrop = UDP2_D; } else { LOG_RESPONSE(ERROR, "Invalid bottle index", BAD_REQUEST); return; @@ -168,8 +172,8 @@ DEF_GCS_HANDLE(Post, targets, locations) { float drop_lat = airdrop_target.coordinate().latitude(); float drop_lng = airdrop_target.coordinate().longitude(); - state->getAirdrop()->send( - makeLatLngPacket(SEND_LATLNG, bottle, TARGET_ACQUIRED, drop_lat, drop_lng, curr_alt_m)); + state->getAirdrop()->send(makeLatLngPacket(SEND_LATLNG, airdrop, TARGET_ACQUIRED, drop_lat, + drop_lng, curr_alt_m)); } LOG_RESPONSE(INFO, "Uploaded airdrop targets coordinates", OK); } @@ -275,36 +279,25 @@ DEF_GCS_HANDLE(Get, camera, capture) { DEF_GCS_HANDLE(Post, dodropnow) { LOG_REQUEST("POST", "/dodropnow"); + LOG_F(INFO, "Received signal to drop current bottle "); - BottleSwap bottle_proto; - google::protobuf::util::JsonStringToMessage(request.body, &bottle_proto); - - bottle_t bottle; - if (bottle_proto.index() == BottleDropIndex::A) { - bottle = UDP2_A; - } else if (bottle_proto.index() == BottleDropIndex::B) { - bottle = UDP2_B; - } else if (bottle_proto.index() == BottleDropIndex::C) { - bottle = UDP2_C; - } else if (bottle_proto.index() == BottleDropIndex::D) { - bottle = UDP2_D; - } else if (bottle_proto.index() == BottleDropIndex::E) { - bottle = UDP2_E; - } else { - LOG_RESPONSE(ERROR, "Invalid bottle index", BAD_REQUEST); + if (state->getMav() == nullptr) { + LOG_RESPONSE(ERROR, "Mavlink not connected", BAD_REQUEST); return; } - LOG_F(INFO, "Received signal to drop bottle %d", bottle); + // Copied from the integration test + std::optional next_airdrop_to_drop; + AirdropIndex next_airdrop = static_cast(2); + next_airdrop_to_drop = static_cast(next_airdrop); - if (state->getAirdrop() == nullptr) { - LOG_RESPONSE(ERROR, "Airdrop not connected", BAD_REQUEST); - return; + // std::string message; + // drop + if (triggerAirdrop(state->getMav() , next_airdrop_to_drop.value())) { + LOG_RESPONSE(INFO, "Dropped Bottle Successfully", OK); + } else { + LOG_RESPONSE(INFO, "Failed to drop bottle", INTERNAL_SERVER_ERROR); } - - state->getAirdrop()->send(makeDropNowPacket(bottle)); - - LOG_RESPONSE(INFO, "Dropped bottle", OK); } DEF_GCS_HANDLE(Post, takeoff, manual) { @@ -331,264 +324,229 @@ DEF_GCS_HANDLE(Post, takeoff, autonomous) { LOG_RESPONSE(INFO, "Set status of WaitForTakeoff Tick to autonomous", OK); } -DEF_GCS_HANDLE(Post, targets, validate) { - LOG_REQUEST("POST", "/targets/validate"); - auto lock_ptr = state->getTickLockPtr(); +DEF_GCS_HANDLE(Get, targets, all) { + LOG_REQUEST("GET", "/targets/all"); - if (!lock_ptr.has_value()) { - LOG_RESPONSE(WARNING, "Not currently in CVLoiter Tick", BAD_REQUEST); + auto aggregator = state->getCV(); + if (!aggregator) { + LOG_RESPONSE(ERROR, "CV not connected yet", BAD_REQUEST); return; } - lock_ptr->data->setStatus(CVLoiterTick::Status::Validated); - LOG_RESPONSE(INFO, "Set status of CVLoiter Tick to validated", OK); -} - -DEF_GCS_HANDLE(Post, targets, reject) { - LOG_REQUEST("POST", "/targets/reject"); - auto lock_ptr = state->getTickLockPtr(); - - if (!lock_ptr.has_value()) { - LOG_RESPONSE(WARNING, "Not currently in CVLoiter Tick", BAD_REQUEST); + // 1) Lock aggregator & pop all new runs + std::vector new_runs = aggregator->popAllRuns(); + if (new_runs.empty()) { + // No new data + LOG_RESPONSE(INFO, "No new runs found", OK, "[]", mime::json); return; } - lock_ptr->data->setStatus(CVLoiterTick::Status::Rejected); - LOG_RESPONSE(INFO, "Set status of CVLoiter Tick to rejected", OK); + // 2) Convert each AggregatedRun into ONE IdentifiedTarget proto + std::vector out_data; + out_data.reserve(new_runs.size()); // Reserve space for efficiency + + for (const auto& run : new_runs) { + // Create ONE IdentifiedTarget message per AggregatedRun + IdentifiedTarget target; + + // Set the run ID + target.set_run_id(run.run_id); + + // Convert the annotated image to base64 and set it (once per run) + std::string b64 = cvMatToBase64(run.annotatedImage); + target.set_picture(b64); + + // Ensure coords and bboxes vectors are the same size (should be guaranteed by Aggregator + // logic) + if (run.coords.size() != run.bboxes.size()) { + LOG_F(ERROR, + "Mismatch between coordinates (%ld) and bboxes (%ld) count in run_id %d. " + "Skipping this run.", + run.coords.size(), run.bboxes.size(), run.run_id); + continue; // Skip this problematic run + } + + // Add all coordinates and bounding boxes from this run + for (size_t i = 0; i < run.bboxes.size(); ++i) { + // Add coordinate + GPSCoord* proto_coord = target.add_coordinates(); // Use the plural field name + proto_coord->set_latitude(run.coords[i].latitude()); + proto_coord->set_longitude(run.coords[i].longitude()); + proto_coord->set_altitude(run.coords[i].altitude()); + + // Add bounding box + BboxProto* proto_bbox = target.add_bboxes(); // Use the plural field name + proto_bbox->set_x1(run.bboxes[i].x1); + proto_bbox->set_y1(run.bboxes[i].y1); + proto_bbox->set_x2(run.bboxes[i].x2); + proto_bbox->set_y2(run.bboxes[i].y2); + } + + // Add the completed IdentifiedTarget (representing the whole run) to the output list + out_data.push_back(std::move(target)); + } // End loop over AggregatedRuns + + // 3) Serialize the vector of IdentifiedTarget messages to JSON + // Ensure messagesToJson can handle a vector or use iterators correctly + std::string out_data_json = messagesToJson(out_data.begin(), out_data.end()); + LOG_RESPONSE(INFO, "Returning newly aggregated runs", OK, out_data_json.c_str(), mime::json); } -DEF_GCS_HANDLE(Get, targets, all) { - LOG_REQUEST("GET", "/targets/all"); +// *** NEW Handler for POST /targets/matched *** +DEF_GCS_HANDLE(Post, targets, matched) { + LOG_REQUEST("POST", "/targets/matched"); if (state->getCV() == nullptr) { - LOG_RESPONSE(ERROR, "CV not connected yet", BAD_REQUEST); + LOG_RESPONSE(ERROR, "CV not init yet", BAD_REQUEST); return; } - LockPtr results = state->getCV()->getResults(); + nlohmann::json j_root = nlohmann::json::parse(request.body); - // Convert to protobuf serialization - std::vector out_data; - int id = 0; // id of the target is the index in the detected_targets vector - // See layout of Identified target proto here: - // https://github.com/tritonuas/protos/blob/master/obc.proto - for (auto& target : results.data->detected_targets) { - IdentifiedTarget out; - out.set_id(id); - out.set_picture(cvMatToBase64(target.crop.croppedImage)); - out.set_ismannikin(target.crop.isMannikin); + LOG_S(INFO) << j_root; - GPSCoord* coord = new GPSCoord; - coord->set_altitude(target.coord.altitude()); - coord->set_longitude(target.coord.longitude()); - coord->set_latitude(target.coord.latitude()); - out.set_allocated_coordinate(coord); // will be freed in destructor + LockPtr matched_results = state->getCV()->getMatchedResults(); - out_data.push_back(std::move(out)); + if (matched_results.data == nullptr) { + LOG_S(ERROR) << "lockptr is null"; + } - // not setting target info because that doesn't really make sense with the current context - // of the matching algorithm, since the algorithm is matching cropped targets to the - // expected not-stolen generated images, so it isn't really classifying targets with a - // specific alphanumeric, color, etc... + AirdropTarget returned_matched_result; - id++; + for (const auto& instance : j_root) { + LOG_S(INFO) << returned_matched_result.index(); + google::protobuf::util::JsonStringToMessage(instance.dump(), &returned_matched_result); + LOG_S(WARNING) << returned_matched_result.index(); + matched_results.data->matched_airdrop[returned_matched_result.index()] + = returned_matched_result; + LOG_S(ERROR) << returned_matched_result.index(); } - std::string out_data_json = messagesToJson(out_data.begin(), out_data.end()); - LOG_RESPONSE(INFO, "Got serialized target data", OK, out_data_json.c_str(), mime::json); -} - -DEF_GCS_HANDLE(Get, targets, matched) { - try { - LOG_REQUEST("GET", "/targets/matched"); - if (state->getCV() == nullptr) { - LOG_RESPONSE(ERROR, "CV not connected yet", BAD_REQUEST); - return; - } + auto lock_ptr = state->getTickLockPtr(); + if (!lock_ptr.has_value()) { + LOG_RESPONSE(WARNING, "Not currently in Loiter Tick", BAD_REQUEST); + return; + } + lock_ptr->data->setStatus(CVLoiterTick::Status::Validated); - /* - NOTE / TODO: - ok so these protobuf messages should really be refactored a bit - currently the MatchedTarget protobuf message contains both a Bottle and - IdentifiedTarget, which in theory seems fine but gets annoying because - now to make the message to send down here we have to construct an entire IdentifiedTarget - struct and then send that down, when really we should only need to send the id - down because all of the IdentifiedTarget information should have been sent - in the GET /targets/all endpoint - - - tyler - */ - - // this vector of bottles needs to live as long as this function because - // we need to give a ptr to these bottles when constructing the MatchedTarget protobuf, - // and we don't want that data to go out of scope and become garbage - std::vector bottles = state->mission_params.getAirdropBottles(); - - // convert to protobuf serialization - std::vector out_data; - - LockPtr results = state->getCV()->getResults(); - for (const auto& [bottle, target_index] : results.data->matches) { - if (!target_index.has_value()) { - continue; - } - - if (target_index.value() >= results.data->detected_targets.size()) { - LOG_RESPONSE(ERROR, "Out of bounds match error", INTERNAL_SERVER_ERROR); - return; - } - - LOG_F(INFO, "bottle %d matched with target %ld", - static_cast(bottle), target_index.value()); - - const DetectedTarget& detected_target = - results.data->detected_targets.at(target_index.value()); - - MatchedTarget matched_target; - Bottle* curr_bottle = nullptr; - for (auto& b : bottles) { - if (b.index() == bottle) { - curr_bottle = new Bottle; - // this is soooo goooooood :0 - curr_bottle->set_index(b.index()); - curr_bottle->set_alphanumeric(b.alphanumeric()); - curr_bottle->set_alphanumericcolor(b.alphanumericcolor()); - curr_bottle->set_shape(b.shape()); - curr_bottle->set_shapecolor(b.shapecolor()); - } - } - if (curr_bottle == nullptr) { - LOG_F(WARNING, "Unmatched bottle"); - continue; - } - matched_target.set_allocated_bottle(curr_bottle); // freed in destructor - - auto identified_target = new IdentifiedTarget; - - identified_target->set_id(target_index.value()); - identified_target->set_alphanumeric(curr_bottle->alphanumeric()); - identified_target->set_alphanumericcolor(curr_bottle->alphanumericcolor()); - identified_target->set_shape(curr_bottle->shape()); - identified_target->set_shapecolor(curr_bottle->shapecolor()); - - matched_target.set_allocated_target(identified_target); // will be freed in destructor - - out_data.push_back(matched_target); - } + LOG_RESPONSE(INFO, "Finished setting targets (and thus loitering)", OK); +} - for (auto& matched_target : out_data) { - LOG_F(INFO, "bottle %d matched to target %d", - static_cast(matched_target.bottle().index()), - matched_target.target().id()); - } +DEF_GCS_HANDLE(Post, kill, kill, kill) { + LOG_REQUEST("POST", "/kill/kill/kill"); - std::string out_data_json = messagesToJson(out_data.begin(), out_data.end()); - LOG_RESPONSE(INFO, "Got serialized target match data", OK, - out_data_json.c_str(), mime::json); - } - catch (std::exception ex) { - LOG_RESPONSE(ERROR, "Who fucking knows what just happened", INTERNAL_SERVER_ERROR); + if (state->getMav() != nullptr) { + state->getMav()->KILL_THE_PLANE_DO_NOT_CALL_THIS_ACCIDENTALLY(); + LOG_RESPONSE(ERROR, "Attempted to kill the plane", OK); + } else { + LOG_RESPONSE(ERROR, "Cannot kill the plane: no mav connection", BAD_REQUEST); } } -DEF_GCS_HANDLE(Post, targets, matched) { - try { - LOG_REQUEST("POST", "/targets/matched"); - - // not using protobufs cause that shit is NOT working - // json string will be in the format - // {"A": 3} +DEF_GCS_HANDLE(Post, camera, startstream) { + LOG_REQUEST("POST", "/camera/startstream"); - json j = json::parse(request.body); + std::shared_ptr cam = state->getCamera(); + // const string + uint32_t interval; + unsigned long parsed_ul = std::stoul(request.body); // NOLINT + interval = static_cast(parsed_ul); + std::chrono::milliseconds chrono_interval(interval); - if (state->getCV() == nullptr) { - LOG_RESPONSE(ERROR, "CV not init yet", BAD_REQUEST); + if (!cam->isConnected()) { + LOG_F(INFO, "Camera not connected. Attempting to connect..."); + cam->connect(); + if (!cam->isConnected()) { + LOG_F(ERROR, "Failed to connect to the camera after connection attempt."); + LOG_RESPONSE(ERROR, "Failed to connect to camera", NOT_FOUND); return; } + LOG_F(INFO, "Camera connected successfully."); + } - LockPtr results = state->getCV()->getResults(); + cam->startStreaming(); + cam->startTakingPictures(chrono_interval, state->getMav()); - LOG_S(INFO) << j; + LOG_RESPONSE(INFO, "Started Camera Stream", OK); +} - for (auto& [key, val] : j.items()) { - std::cout << "key: " << key << ", val: " << val << std::endl; - } +DEF_GCS_HANDLE(Post, camera, endstream) { + LOG_REQUEST("POST", "/camera/endstream"); - // obviously this should be cleaned up, but it should work for now - if (j.contains("A")) { - int id = j.at("A"); - LOG_F(INFO, "Updating bottle A to id %d", id); - results.data->matches[BottleDropIndex::A] = static_cast(id); - } - if (j.contains("B")) { - int id = j.at("B"); - LOG_F(INFO, "Updating bottle B to id %d", id); - results.data->matches[BottleDropIndex::B] = static_cast(id); - } - if (j.contains("C")) { - int id = j.at("C"); - LOG_F(INFO, "Updating bottle C to id %d", id); - results.data->matches[BottleDropIndex::C] = static_cast(id); - } - if (j.contains("D")) { - int id = j.at("D"); - LOG_F(INFO, "Updating bottle D to id %d", id); - results.data->matches[BottleDropIndex::D] = static_cast(id); + std::shared_ptr cam = state->getCamera(); + + if (!cam->isConnected()) { + LOG_F(INFO, "Camera not connected. Attempting to connect..."); + cam->connect(); + if (!cam->isConnected()) { + LOG_F(ERROR, "Failed to connect to the camera after connection attempt."); + LOG_RESPONSE(ERROR, "Failed to connect to camera", NOT_FOUND); + return; } - if (j.contains("E")) { - int id = j.at("E"); - LOG_F(INFO, "Updating bottle E to id %d", id); - results.data->matches[BottleDropIndex::E] = static_cast(id); + LOG_F(INFO, "Camera connected successfully."); + } + + cam->stopTakingPictures(); + + std::deque images; + images = cam->getAllImages(); + for (const ImageData& image : images) { + std::filesystem::path save_dir = state->config.camera.save_dir; + std::filesystem::path img_filepath = save_dir / (std::to_string(image.TIMESTAMP) + + std::string(".jpg")); + std::filesystem::path json_filepath = save_dir / (std::to_string(image.TIMESTAMP) + + std::string(".json")); + saveImageToFile(image.DATA, img_filepath); + if (image.TELEMETRY.has_value()) { + saveImageTelemetryToFile(image.TELEMETRY.value(), json_filepath); } - - LOG_RESPONSE(INFO, "Updated bottle matchings", OK); - } - catch (std::exception ex) { - LOG_RESPONSE(ERROR, "Who fucking knows what just happened", INTERNAL_SERVER_ERROR); + LOG_F(INFO, "Saving image %s", img_filepath.string().c_str()); } + + LOG_RESPONSE(INFO, "Ended Camera Stream", OK); } -DEF_GCS_HANDLE(Post, kill, kill, kill) { - LOG_REQUEST("POST", "/kill/kill/kill"); +DEF_GCS_HANDLE(Get, tickstate) { + // Not using the macros here so that it doesn't scream at you every 1 second + // LOG_REQUEST("GET", "/tickstate"); - if (state->getMav() != nullptr) { - state->getMav()->KILL_THE_PLANE_DO_NOT_CALL_THIS_ACCIDENTALLY(); - LOG_RESPONSE(ERROR, "Attempted to kill the plane", OK); - } else { - LOG_RESPONSE(ERROR, "Cannot kill the plane: no mav connection", BAD_REQUEST); - } + TickID tickID = state->getTickID(); + std::string tick_state = TICK_ID_TO_STR(tickID); + + // LOG_RESPONSE(INFO, "Returning tick state", OK, tick_state, mime::plaintext); + response.set_content(tick_state, mime::plaintext); + response.status = OK; } -DEF_GCS_HANDLE(Get, oh, shit) { - LOG_REQUEST("GET", "/oh/shit"); +// DEF_GCS_HANDLE(Get, oh, shit) { +// LOG_REQUEST("GET", "/oh/shit"); - if (state->getCV() == nullptr) { - LOG_RESPONSE(ERROR, "No CV yet :(", BAD_REQUEST); - return; - } +// if (state->getCV() == nullptr) { +// LOG_RESPONSE(ERROR, "No CV yet :(", BAD_REQUEST); +// return; +// } - GPSCoord center; - center.set_latitude(38.31440741337194); - center.set_longitude(-76.54460728168489); - center.set_altitude(0); +// GPSCoord center; +// center.set_latitude(38.31440741337194); +// center.set_longitude(-76.54460728168489); +// center.set_altitude(0); - LockPtr results = state->getCV()->getResults(); +// LockPtr results = state->getCV()->getResults(); - for (int i = 1; i <= 5; i++) { - CroppedTarget crop; - crop.isMannikin = false; - crop.croppedImage = cv::Mat(cv::Size(20, 20), CV_8UC3, cv::Scalar(255)); +// for (int i = 1; i <= 5; i++) { +// CroppedTarget crop; +// crop.isMannikin = false; +// crop.croppedImage = cv::Mat(cv::Size(20, 20), CV_8UC3, cv::Scalar(255)); - DetectedTarget target(center, static_cast(i), 100.0, crop); - target.coord = center; - target.likely_bottle = static_cast(i); - target.crop = crop; +// DetectedTarget target(center, static_cast(i), 100.0, crop); +// target.coord = center; +// target.likely_bottle = static_cast(i); +// target.crop = crop; - results.data->detected_targets.push_back(target); - } - - LOG_RESPONSE(INFO, "Oh shit", OK); -} +// results.data->detected_targets.push_back(target); +// } +// LOG_RESPONSE(INFO, "Oh shit", OK); +// } diff --git a/src/network/mavlink.cpp b/src/network/mavlink.cpp index 193f8252..3a234f5e 100644 --- a/src/network/mavlink.cpp +++ b/src/network/mavlink.cpp @@ -2,10 +2,10 @@ #include #include -#include #include #include #include +#include #include #include @@ -80,8 +80,7 @@ MavlinkClient::MavlinkClient(OBCConfig config) // require a recompile to change the typing of a specific parameter if you // get it wrong. auto result = mavsdk::Param::Result::Unknown; - if (param == "FS_LONG_TIMEOUT" || - param == "AFS_RC_FAIL_TIME" || + if (param == "FS_LONG_TIMEOUT" || param == "AFS_RC_FAIL_TIME" || param == "FS_SHORT_TIMEOUT") { result = this->param->set_param_float(param, val); } else { @@ -204,9 +203,36 @@ MavlinkClient::MavlinkClient(OBCConfig config) // }); } +// Implement the triggerRelay method +bool MavlinkClient::triggerRelay(int relay_number, bool state) { + if (!this->passthrough) { + LOG_F(ERROR, "MavlinkPassthrough interface not available"); + return false; + } + + // Command for DO_SET_RELAY (command ID 181) + // Docs for command: https://mavlink.io/en/messages/common.html#MAV_CMD_DO_SET_RELAY + mavsdk::MavlinkPassthrough::CommandLong command{}; + command.command = 181; // MAV_CMD_DO_SET_RELAY + command.param1 = static_cast(relay_number); // Relay instance (0-based) + command.param2 = state ? 0.0f : 1.0f; // 1=on, 0=off + + LOG_F(INFO, "Sending DO_SET_RELAY command to trigger relay %d (state: %s)", relay_number, + state ? "ON" : "OFF"); + + auto result = this->passthrough->send_command_long(command); + + if (result == mavsdk::MavlinkPassthrough::Result::Success) { + LOG_F(INFO, "Successfully sent relay command"); + return true; + } else { + LOG_F(ERROR, "Failed to send relay command: %d", static_cast(result)); + return false; + } +} + bool MavlinkClient::uploadMissionUntilSuccess(std::shared_ptr state, - bool upload_geofence, - const MissionPath& path) const { + bool upload_geofence, const MissionPath& path) const { if (upload_geofence) { if (!this->uploadGeofenceUntilSuccess(state)) { return false; @@ -282,7 +308,6 @@ bool MavlinkClient::uploadWaypointsUntilSuccess(std::shared_ptr st const MissionPath& waypoints) const { LOG_SCOPE_F(INFO, "Uploading waypoints"); - while (true) { LOG_F(INFO, "Sending waypoint information..."); @@ -379,9 +404,7 @@ mavsdk::Telemetry::FlightMode MavlinkClient::flight_mode() { return this->data.flight_mode; } -int32_t MavlinkClient::curr_waypoint() const { - return this->mission->mission_progress().current; -} +int32_t MavlinkClient::curr_waypoint() const { return this->mission->mission_progress().current; } bool MavlinkClient::isMissionFinished() { // Boolean representing if mission is finished diff --git a/src/pathing/CMakeLists.txt b/src/pathing/CMakeLists.txt index 243777f0..97bf7d8e 100644 --- a/src/pathing/CMakeLists.txt +++ b/src/pathing/CMakeLists.txt @@ -33,5 +33,6 @@ target_add_httplib(${LIB_NAME}) target_add_mavsdk(${LIB_NAME}) target_add_matplot(${LIB_NAME}) target_add_loguru(${LIB_NAME}) +target_add_onnxruntime(${LIB_NAME}) target_include_directories(${LIB_NAME} PRIVATE ${ImageMagick_INCLUDE_DIRS}) target_link_libraries(${LIB_NAME} PRIVATE -Wl,--copy-dt-needed-entries ${ImageMagick_LIBRARIES}) \ No newline at end of file diff --git a/src/pathing/static.cpp b/src/pathing/static.cpp index e9abbe5a..c56603f9 100644 --- a/src/pathing/static.cpp +++ b/src/pathing/static.cpp @@ -451,10 +451,11 @@ std::vector HoverCoveragePathing::run() { this->drop_zone.size()); } - XYZCoord top_left = this->drop_zone.at(3); - XYZCoord top_right = this->drop_zone.at(2); + // Input Coordinates MUST BE in this order XYZCoord bottom_left = this->drop_zone.at(0); XYZCoord bottom_right = this->drop_zone.at(1); + XYZCoord top_right = this->drop_zone.at(2); + XYZCoord top_left = this->drop_zone.at(3); std::vector hover_points; @@ -545,11 +546,11 @@ std::vector> generateGoalListDeviations(const std::vector< } std::vector> generateRankedNewGoalsList(const std::vector &goals, - const Environment &airspace) { + const Environment &mapping_bounds) { // generate deviation points randomly in the mapping region std::vector deviation_points; for (int i = 0; i < 200; i++) { - deviation_points.push_back(airspace.getRandomPoint(true)); + deviation_points.push_back(mapping_bounds.getRandomPoint(true)); printf("Deviation point: %f, %f\n", deviation_points.back().x, deviation_points.back().y); } @@ -565,7 +566,7 @@ std::vector> generateRankedNewGoalsList(const std::vector< // run each goal list and get the area covered and the length of the path std::vector> area_length_pairs; for (const std::vector &new_goals : new_goals_list) { - area_length_pairs.push_back(airspace.estimateAreaCoveredAndPathLength(new_goals)); + area_length_pairs.push_back(mapping_bounds.estimateAreaCoveredAndPathLength(new_goals)); } // rank the new goal lists by the area covered and the length of the path @@ -607,9 +608,10 @@ MissionPath generateInitialPath(std::shared_ptr state) { } // update goals here - // create a dummy environment - Environment airspace(state->mission_params.getFlightBoundary(), {}, {}, goals, {}); - goals = generateRankedNewGoalsList(goals, airspace)[0]; + if (state->config.pathing.rrt.generate_deviations) { + Environment mapping_bounds(state->mission_params.getMappingBoundary(), {}, {}, goals, {}); + goals = generateRankedNewGoalsList(goals, mapping_bounds)[0]; + } double init_angle = std::atan2(goals.front().y - state->mission_params.getWaypoints().front().y, @@ -633,17 +635,29 @@ MissionPath generateInitialPath(std::shared_ptr state) { } MissionPath generateSearchPath(std::shared_ptr state) { + std::vector gps_coords; if (state->config.pathing.coverage.method == AirdropCoverageMethod::Enum::FORWARD) { LOG_F(FATAL, "Forward search path not fully integrated yet."); + + RRTPoint start(state->mission_params.getWaypoints().front(), 0); + + // TODO , change the starting point to be something closer to loiter + // region + ForwardCoveragePathing pathing(start, SEARCH_RADIUS, + state->mission_params.getFlightBoundary(), + state->mission_params.getAirdropBoundary(), state->config); + + for (const auto &coord : pathing.run()) { + gps_coords.push_back(state->getCartesianConverter()->toLatLng(coord)); + } + return MissionPath(MissionPath::Type::FORWARD, {}); } else { // hover HoverCoveragePathing pathing(state); - std::vector gps_coords; for (const auto &coord : pathing.run()) { gps_coords.push_back(state->getCartesianConverter()->toLatLng(coord)); } - return MissionPath(MissionPath::Type::HOVER, gps_coords, state->config.pathing.coverage.hover.hover_time_s); } diff --git a/src/ticks/CMakeLists.txt b/src/ticks/CMakeLists.txt index 6ad68602..616c5804 100644 --- a/src/ticks/CMakeLists.txt +++ b/src/ticks/CMakeLists.txt @@ -16,6 +16,7 @@ set(FILES takeoff.cpp active_takeoff.cpp wait_for_takeoff.cpp + refueling.cpp ) set(LIB_DEPS @@ -45,5 +46,6 @@ target_add_httplib(${LIB_NAME}) target_add_mavsdk(${LIB_NAME}) target_add_matplot(${LIB_NAME}) target_add_loguru(${LIB_NAME}) +target_add_onnxruntime(${LIB_NAME}) target_include_directories(${LIB_NAME} PRIVATE ${ImageMagick_INCLUDE_DIRS}) target_link_libraries(${LIB_NAME} PRIVATE -Wl,--copy-dt-needed-entries ${ImageMagick_LIBRARIES}) \ No newline at end of file diff --git a/src/ticks/active_takeoff.cpp b/src/ticks/active_takeoff.cpp index e1007533..4b7b93ab 100644 --- a/src/ticks/active_takeoff.cpp +++ b/src/ticks/active_takeoff.cpp @@ -5,6 +5,7 @@ #include "ticks/ids.hpp" #include "utilities/constants.hpp" +#include "ticks/airdrop_prep.hpp" #include "ticks/fly_waypoints.hpp" #include "ticks/mav_upload.hpp" #include "ticks/mission_prep.hpp" @@ -50,7 +51,13 @@ Tick* ActiveTakeoffTick::tick() { // NOTE: keep in sync with manual takeoff tick // transitions to flying waypoints tick, such that when the flying waypoints // tick is done it transitions to uploading the coverage path - return new FlyWaypointsTick(this->state, new MavUploadTick( - this->state, new FlySearchTick(this->state), - state->getCoveragePath(), false)); + + if (state->getDroppedAirdrops().size() >= this->state->config.takeoff.payload_size) { + return new FlyWaypointsTick(this->state, new AirdropPrepTick(this->state)); + // return new AirdropPrepTick(this->state); + } else { + return new FlyWaypointsTick(this->state, new MavUploadTick( + this->state, new FlySearchTick(this->state), + state->getCoveragePath(), false)); + } } diff --git a/src/ticks/airdrop_approach.cpp b/src/ticks/airdrop_approach.cpp index 071b7f6b..2f59e2fb 100644 --- a/src/ticks/airdrop_approach.cpp +++ b/src/ticks/airdrop_approach.cpp @@ -1,43 +1,92 @@ #include "ticks/airdrop_approach.hpp" +#include + +#include #include +#include -#include "ticks/ids.hpp" -#include "ticks/mav_upload.hpp" -#include "ticks/fly_waypoints.hpp" #include "ticks/airdrop_prep.hpp" +#include "ticks/fly_waypoints.hpp" +#include "ticks/ids.hpp" #include "ticks/manual_landing.hpp" +#include "ticks/mav_upload.hpp" #include "utilities/constants.hpp" +#include "ticks/refueling.hpp" +#include "ticks/wait_for_takeoff.hpp" + AirdropApproachTick::AirdropApproachTick(std::shared_ptr state) - :Tick(state, TickID::AirdropApproach) {} + : Tick(state, TickID::AirdropApproach) {} void AirdropApproachTick::init() { - state->getMav()->startMission(); + LOG_F(INFO, "start mission airdrop"); + this->state->getMav()->startMission(); } std::chrono::milliseconds AirdropApproachTick::getWait() const { return AIRDROP_APPROACH_TICK_WAIT; } +// Helper function to trigger the airdrop mechanism +bool triggerAirdrop(std::shared_ptr mav, airdrop_t airdrop_index) { + LOG_F(INFO, "Triggering airdrop mechanism for airdrop %d", static_cast(airdrop_index)); + + // Use the new triggerRelay method to activate RELAY2 (relay index 1) + // For ArduPilot, RELAY2 corresponds to relay_number = 1 (zero-indexed) + bool success = mav->triggerRelay(1, true); + + if (success) { + LOG_F(INFO, "Successfully activated RELAY2 for airdrop"); + + // Sleep briefly to ensure the relay has time to activate + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + + // Optional: Turn off the relay after a delay + // This depends on your specific hardware setup - some relays may need to be turned off + // after activation, while others might reset automatically + if (mav->triggerRelay(1, false)) { + LOG_F(INFO, "Successfully deactivated RELAY2 after airdrop"); + } else { + LOG_F(WARNING, "Failed to deactivate RELAY2 after airdrop"); + } + } else { + LOG_F(ERROR, "Failed to activate RELAY2 for airdrop"); + } + + return success; +} + Tick* AirdropApproachTick::tick() { if (state->getMav()->isAtFinalWaypoint()) { - if (state->next_bottle_to_drop.has_value()) { - LOG_F(INFO, "Dropping bottle %d", state->next_bottle_to_drop.value()); - state->getAirdrop()->send(makeDropNowPacket(state->next_bottle_to_drop.value())); - state->getAirdrop()->send(makeDropNowPacket(state->next_bottle_to_drop.value())); - state->getAirdrop()->send(makeDropNowPacket(state->next_bottle_to_drop.value())); + if (state->next_airdrop_to_drop.has_value()) { + LOG_F(INFO, "Dropping airdrop %d", state->next_airdrop_to_drop.value()); + + // Trigger the airdrop servo/relay + triggerAirdrop(state->getMav(), state->next_airdrop_to_drop.value()); + + // markAirdropAsDropped() has already been called in airdrop_prep, so don't update here + // Lowkey kinda bad design but I didn't write this so don't blame me + // If you want to uncomment the following lines to test if this shit works, make sure + // to comment the ones in airdrop_prep + + // Convert airdrop_t to AirdropIndex when calling markAirdropAsDropped + // state->markAirdropAsDropped( + // static_cast(state->next_airdrop_to_drop.value() - 1)); + } else { LOG_F(ERROR, "Cannot drop bottle because no bottle to drop"); } } if (state->getMav()->isMissionFinished()) { - if (state->getDroppedBottles().size() >= NUM_AIRDROP_BOTTLES) { - return new ManualLandingTick(state); + if (state->getDroppedAirdrops().size() >= NUM_AIRDROPS) { + return new ManualLandingTick(state, nullptr); + } else if (state->getDroppedAirdrops().size() % state->config.takeoff.payload_size == 0) { + return new ManualLandingTick(state, new RefuelingTick(state)); } else { - return new MavUploadTick(state, new FlyWaypointsTick(state, - new AirdropPrepTick(state)), state->getInitPath(), false); + return new MavUploadTick(state, new FlyWaypointsTick(state, new AirdropPrepTick(state)), + state->getInitPath(), false); } } diff --git a/src/ticks/airdrop_prep.cpp b/src/ticks/airdrop_prep.cpp index 693baa8a..c22191d6 100644 --- a/src/ticks/airdrop_prep.cpp +++ b/src/ticks/airdrop_prep.cpp @@ -7,9 +7,9 @@ #include "pathing/static.hpp" #include "ticks/airdrop_approach.hpp" #include "ticks/ids.hpp" -#include "ticks/path_gen.hpp" #include "ticks/manual_landing.hpp" #include "ticks/mav_upload.hpp" +#include "ticks/path_gen.hpp" #include "utilities/logging.hpp" AirdropPrepTick::AirdropPrepTick(std::shared_ptr state) @@ -18,51 +18,60 @@ AirdropPrepTick::AirdropPrepTick(std::shared_ptr state) std::chrono::milliseconds AirdropPrepTick::getWait() const { return AIRDROP_PREP_TICK_WAIT; } Tick* AirdropPrepTick::tick() { - BottleDropIndex next_bottle = BottleDropIndex::A; + AirdropIndex next_airdrop = AirdropIndex::Kaz; - auto dropped_bottles = state->getDroppedBottles(); + auto dropped_airdrops = state->getDroppedAirdrops(); - if (dropped_bottles.size() >= NUM_AIRDROP_BOTTLES) { - return new ManualLandingTick(state); + if (dropped_airdrops.size() >= NUM_AIRDROPS) { + return new ManualLandingTick(state, nullptr); } - LockPtr results = state->getCV()->getResults(); + LockPtr results = state->getCV()->getMatchedResults(); - for (int i = BottleDropIndex::A; i <= BottleDropIndex::E; i++) { - if (dropped_bottles.contains(static_cast(i))) { + for (int i = AirdropIndex::Kaz; i <= AirdropIndex::Daniel; i++) { + if (dropped_airdrops.contains(static_cast(i))) { continue; } - next_bottle = static_cast(i); + next_airdrop = static_cast(i); - if (!results.data->matches.at(next_bottle).has_value()) { - LOG_F(INFO, "Skipping bottle %d because we didn't match it", - static_cast(next_bottle)); - state->markBottleAsDropped(next_bottle); - continue; - } + // Dont think we need this, as everything theoretically should be matched when + // the matched route is posted. + // if (!results.data->matches.at(next_airdrop).has_value()) { + // LOG_F(INFO, "Skipping bottle %d because we didn't match it", + // static_cast(next_bottle)); + // state->markAirdropAsDropped(next_bottle); + // continue; + // } break; } - state->markBottleAsDropped(next_bottle); + state->markAirdropAsDropped(next_airdrop); // The or condition here shouldn't be met because above we check for value // before setting next_bottle. // But just in case we default to whatever location target 0 was found at. - auto target = results.data->detected_targets.at( - results.data->matches.at(next_bottle).value_or(0)); + // auto target = + // results.data->detected_targets.at(results.data->matches.at(next_airdrop).value_or(0)); + auto target = results.data->matched_airdrop.at(next_airdrop); // IMPORTANT: need to set the altitude of the target coord to the config value so it doesn't // try and nosedive into the ground... - target.coord.set_altitude(state->config.pathing.approach.drop_altitude_m); + target.mutable_coordinate()->set_altitude(state->config.pathing.approach.drop_altitude_m); + + // Use for testing cuz testing images don't have proper GPS coordinates attached to them + // Uncomment the following two lines for sitl testing + // target.mutable_coordinate()->set_latitude(38.31584541086285); + // target.mutable_coordinate()->set_longitude(-76.55316855169548); - LOG_F(INFO, "Routing to airdrop target %d at (%f, %f) alt %f", static_cast(next_bottle), - target.coord.latitude(), target.coord.longitude(), target.coord.altitude()); + LOG_F(INFO, "Routing to airdrop target %d at (%f, %f) alt %f", static_cast(next_airdrop), + target.coordinate().latitude(), target.coordinate().longitude(), + target.coordinate().altitude()); - state->setAirdropPath(generateAirdropApproach(state, target.coord)); + state->setAirdropPath(generateAirdropApproach(state, target.coordinate())); LOG_F(INFO, "Generated approach path"); - state->next_bottle_to_drop = static_cast(next_bottle); + state->next_airdrop_to_drop = static_cast(next_airdrop); // If we ever switch to use the actual guided part of the protocol probably want // to uncomment these out and change where currently we are sending the do drop now command @@ -71,8 +80,9 @@ Tick* AirdropPrepTick::tick() { // DISARM, UDP2_ALL, OBC_NULL, state->getMav()->altitude_agl_m())); // state->getAirdrop()->send(makeArmPacket( - // ARM, static_cast(next_bottle), OBC_NULL, state->getMav()->altitude_agl_m())); + // ARM, static_cast(next_bottle), OBC_NULL, + // state->getMav()->altitude_agl_m())); return new MavUploadTick(this->state, new AirdropApproachTick(this->state), - state->getAirdropPath(), false); + state->getAirdropPath(), false); } diff --git a/src/ticks/cv_loiter.cpp b/src/ticks/cv_loiter.cpp index 3dff1841..bc255f1c 100644 --- a/src/ticks/cv_loiter.cpp +++ b/src/ticks/cv_loiter.cpp @@ -40,43 +40,45 @@ Tick* CVLoiterTick::tick() { // Check status of the CV Results if (status == Status::Validated) { - const std::array ALL_BOTTLES = { - BottleDropIndex::A, BottleDropIndex::B, BottleDropIndex::C, - BottleDropIndex::D, BottleDropIndex::E + /* + const std::array ALL_AIRDROPS = { + AirdropIndex::Kaz, AirdropIndex::Kimi, AirdropIndex::Chris, + AirdropIndex::Daniel }; + */ LockPtr results = state->getCV()->getResults(); - for (const auto& bottle : ALL_BOTTLES) { - // contains will never be false but whatever - if (results.data->matches.contains(bottle)) { - auto opt = results.data->matches.at(bottle); - if (!opt.has_value()) { - continue; - } - - std::size_t index = opt.value(); - - if (index >= results.data->detected_targets.size()) { - continue; - } - - auto target = results.data->detected_targets.at(index); - - float alt = state->getMav()->altitude_agl_m(); - - LOG_F(INFO, "Sending coord(%f, %f) alt %f to bottle %d", - target.coord.latitude(), - target.coord.longitude(), - alt, - static_cast(bottle)); - // assumes that the bottle_t enum in the udp2 stuff is the same as - // BottleDropIndex enum - state->getAirdrop()->send(makeLatLngPacket( - SEND_LATLNG, static_cast(bottle), OBC_NULL, - target.coord.latitude(), target.coord.longitude(), alt)); - } - } + // for (const auto& bottle : ALL_BOTTLES) { + // // contains will never be false but whatever + // if (results.data->matches.contains(bottle)) { + // auto opt = results.data->matches.at(bottle); + // if (!opt.has_value()) { + // continue; + // } + + // std::size_t index = opt.value(); + + // if (index >= results.data->detected_targets.size()) { + // continue; + // } + + // auto target = results.data->detected_targets.at(index); + + // float alt = state->getMav()->altitude_agl_m(); + + // LOG_F(INFO, "Sending coord(%f, %f) alt %f to bottle %d", + // target.coord.latitude(), + // target.coord.longitude(), + // alt, + // static_cast(bottle)); + // // assumes that the bottle_t enum in the udp2 stuff is the same as + // // BottleDropIndex enum + // state->getAirdrop()->send(makeLatLngPacket( + // SEND_LATLNG, static_cast(bottle), OBC_NULL, + // target.coord.latitude(), target.coord.longitude(), alt)); + // } + // } return new AirdropPrepTick(this->state); } else if (status == Status::Rejected) { diff --git a/src/ticks/manual_landing.cpp b/src/ticks/manual_landing.cpp index a40be2df..bacab198 100644 --- a/src/ticks/manual_landing.cpp +++ b/src/ticks/manual_landing.cpp @@ -2,16 +2,59 @@ #include +#include "cv/mapping.hpp" #include "ticks/ids.hpp" #include "utilities/constants.hpp" -ManualLandingTick::ManualLandingTick(std::shared_ptr state) - :Tick(state, TickID::ManualLanding) {} +namespace fs = std::filesystem; -std::chrono::milliseconds ManualLandingTick::getWait() const { - return MANUAL_LANDING_TICK_WAIT; -} +ManualLandingTick::ManualLandingTick(std::shared_ptr state, Tick* next_tick) + :Tick(state, TickID::ManualLanding), next_tick(next_tick) {} + +std::chrono::milliseconds ManualLandingTick::getWait() const { return MANUAL_LANDING_TICK_WAIT; } Tick* ManualLandingTick::tick() { + if (state->getDroppedAirdrops().size() >= NUM_AIRDROPS) { + if (state->getMappingIsDone() == false) { + // Currently does a two pass at the end + // Probably should update for next year to optimize it. + + cv::Stitcher::Mode scan_mode = cv::Stitcher::SCANS; + // Direct stitching of all images in the mapping folder + + fs::path base_dir = "../images/mapping"; + fs::path output_dir = base_dir / "output"; + + // Make sure the output directory exists (create if it doesn't) + if (!fs::exists(output_dir)) { + fs::create_directories(output_dir); + } + + // TODO: Change this to be a config setting later + Mapping mapper; + const int chunk_size = 5; + const int chunk_overlap = 2; + const int max_dim = 3000; + + LOG_F(INFO, "First pass stitching..."); + mapper.firstPass(base_dir.string(), output_dir.string(), chunk_size, + chunk_overlap, scan_mode, max_dim, true); + + LOG_F(INFO, "Second pass stitching..."); + mapper.secondPass(output_dir.string(), scan_mode, max_dim, true); + state->setMappingIsDone(true); + + LOG_F(INFO, "Mapping complete."); + } + } else { + // TODO: Test this in mock testflight + if (!state.get()->getMav()->isArmed()) { + return next_tick; + } + + // Comment the above if and uncomment the below line when testing sitl + // return next_tick; + } + return nullptr; } diff --git a/src/ticks/mission_prep.cpp b/src/ticks/mission_prep.cpp index e17a35d6..8f560477 100644 --- a/src/ticks/mission_prep.cpp +++ b/src/ticks/mission_prep.cpp @@ -1,83 +1,74 @@ #include "ticks/mission_prep.hpp" + #include #include #include -#include "utilities/logging.hpp" #include "core/mission_state.hpp" -#include "ticks/path_gen.hpp" #include "ticks/ids.hpp" +#include "ticks/path_gen.hpp" #include "utilities/datatypes.hpp" +#include "utilities/logging.hpp" MissionPrepTick::MissionPrepTick(std::shared_ptr state) - :Tick(state, TickID::MissionPrep) {} + : Tick(state, TickID::MissionPrep) {} -std::chrono::milliseconds MissionPrepTick::getWait() const { - return MISSION_PREP_TICK_WAIT; -} +std::chrono::milliseconds MissionPrepTick::getWait() const { return MISSION_PREP_TICK_WAIT; } Tick* MissionPrepTick::tick() { if (this->state->mission_params.getCachedMission().has_value()) { LOG_F(INFO, "Valid mission configuration detected"); - std::array bottles_to_drop; + std::array airdrops_to_drop; // Get the airdrop bottles from the mission parameters // and copy into an std::array of the same size. Annoying conversion because // mission parameters and cv aggregator expect them in different data // structures. Could fix by making them both take a vector / both take an // array of size NUM_AIRDROP_BOTTLES - std::copy_n(state->mission_params.getAirdropBottles().begin(), - NUM_AIRDROP_BOTTLES, bottles_to_drop.begin()); + std::copy_n(state->mission_params.getAirdrops().begin(), NUM_AIRDROPS, + airdrops_to_drop.begin()); - std::string matching_model_dir = this->state->config.cv.matching_model_dir; - std::string segmentation_model_dir = this->state->config.cv.segmentation_model_dir; - std::string saliency_model_dir = this->state->config.cv.saliency_model_dir; + std::string yolo_model_dir = this->state->config.cv.yolo_model_dir; LOG_F(INFO, "Instantiating CV Aggregator with the following models:"); - LOG_F(INFO, "Matching Model: %s", matching_model_dir.c_str()); - LOG_F(INFO, "Segmentation Model: %s", segmentation_model_dir.c_str()); - LOG_F(INFO, "Saliency Model: %s", saliency_model_dir.c_str()); + LOG_F(INFO, "Yolo Model: %s", yolo_model_dir.c_str()); // Make a CVAggregator instance and set it in the state - this->state->setCV(std::make_shared(Pipeline( - PipelineParams(bottles_to_drop, - this->generateReferenceImages(bottles_to_drop), - matching_model_dir, - segmentation_model_dir, - saliency_model_dir)))); + this->state->setCV( + std::make_shared(Pipeline(PipelineParams(yolo_model_dir)))); + this->state->setMappingIsDone(false); return new PathGenTick(this->state); } else { return nullptr; } } +std::vector> MissionPrepTick::generateReferenceImages( + std::array competitionObjectives) { + std::vector> ref_imgs; -std::vector> MissionPrepTick::generateReferenceImages - (std::array competitionObjectives) { - std::vector> ref_imgs; - - int curr_bottle_idx = BottleDropIndex::Undefined; - for (const auto& bottle : competitionObjectives) { - curr_bottle_idx++; + // Default is Kaz cuz we don't have Undefined anymore + int curr_airdrop_idx = AirdropIndex::Kaz; + for (const auto& airdrop : competitionObjectives) { + curr_airdrop_idx++; // don't generate reference images for mannikin since matching model doesn't // match mannikins (handled by saliency) - if (bottle.ismannikin()) { - continue; - } + // if (bottle.ismannikin()) { + // continue; + // } httplib::Client client(this->state->config.cv.not_stolen_addr, - this->state->config.cv.not_stolen_port); - auto res = client.Get(this->getNotStolenRoute(bottle)); + this->state->config.cv.not_stolen_port); + auto res = client.Get(this->getNotStolenRoute(airdrop)); // connection failed if (!res) { LOG_F(ERROR, "Failed to send request to not-stolen at %s:%u. Reason: %s", - this->state->config.cv.not_stolen_addr.c_str(), - this->state->config.cv.not_stolen_port, - httplib::to_string(res.error()).c_str()); + this->state->config.cv.not_stolen_addr.c_str(), + this->state->config.cv.not_stolen_port, httplib::to_string(res.error()).c_str()); return {}; } @@ -89,20 +80,11 @@ std::vector> MissionPrepTick::generateRefere cv::Mat data_mat(vectordata, true); cv::Mat ref_img(cv::imdecode(data_mat, 1)); // put 0 if you want greyscale - ref_imgs.push_back({ref_img, (BottleDropIndex)curr_bottle_idx}); + ref_imgs.push_back({ref_img, (AirdropIndex)curr_airdrop_idx}); } return ref_imgs; } -std::string MissionPrepTick::getNotStolenRoute(const Bottle& target) { - std::string char_type = target.alphanumeric(); - std::string char_color = ODLCColorToString(target.alphanumericcolor()); - - std::string shape_type = ODLCShapeToString(target.shape()); - std::string shape_color = ODLCColorToString(target.shapecolor()); - - return std::string("/generate?shape_type=") + shape_type + - std::string("&shape_color=") + shape_color + - std::string("&char_type=") + char_type + - std::string("&char_color=") + char_color; +std::string MissionPrepTick::getNotStolenRoute(const Airdrop& target) { + return std::string("/generate"); } diff --git a/src/ticks/refueling.cpp b/src/ticks/refueling.cpp new file mode 100644 index 00000000..65f036bd --- /dev/null +++ b/src/ticks/refueling.cpp @@ -0,0 +1,22 @@ +#include "ticks/refueling.hpp" + +#include + +#include "ticks/active_takeoff.hpp" +#include "ticks/ids.hpp" +#include "ticks/mav_upload.hpp" +#include "utilities/constants.hpp" + +RefuelingTick::RefuelingTick(std::shared_ptr state) + : Tick(state, TickID::Refueling) {} + +std::chrono::milliseconds RefuelingTick::getWait() const { return REFUELING_TICK_WAIT; } + +Tick* RefuelingTick::tick() { + if (state.get()->getMav()->isArmed()) { + return new MavUploadTick(state, new WaitForTakeoffTick(state), state->getInitPath(), false); + } + + return nullptr; + // return new MavUploadTick(state, new WaitForTakeoffTick(state), state->getInitPath(), false); +} diff --git a/src/ticks/takeoff.cpp b/src/ticks/takeoff.cpp index 3dc126ae..4cfe0560 100644 --- a/src/ticks/takeoff.cpp +++ b/src/ticks/takeoff.cpp @@ -22,6 +22,8 @@ Tick* TakeoffTick::tick() { // NOTE: keep in sync with active_takeoff tick // transitions to flying waypoints tick, such that when the flying waypoints // tick is done it transitions to uploading the coverage path + + // This is MANUAL Takeoff (idk why its named so confusing) return new FlyWaypointsTick(this->state, new MavUploadTick( this->state, new FlySearchTick(this->state), state->getCoveragePath(), false)); diff --git a/src/utilities/CMakeLists.txt b/src/utilities/CMakeLists.txt index 6e879caf..8e60940d 100644 --- a/src/utilities/CMakeLists.txt +++ b/src/utilities/CMakeLists.txt @@ -32,5 +32,6 @@ target_add_httplib(${LIB_NAME}) target_add_mavsdk(${LIB_NAME}) target_add_matplot(${LIB_NAME}) target_add_loguru(${LIB_NAME}) +target_add_onnxruntime(${LIB_NAME}) target_include_directories(${LIB_NAME} PRIVATE ${ImageMagick_INCLUDE_DIRS}) target_link_libraries(${LIB_NAME} PRIVATE -Wl,--copy-dt-needed-entries ${ImageMagick_LIBRARIES}) \ No newline at end of file diff --git a/src/utilities/datatypes.cpp b/src/utilities/datatypes.cpp index b8ce733b..5ae2cc29 100644 --- a/src/utilities/datatypes.cpp +++ b/src/utilities/datatypes.cpp @@ -101,32 +101,29 @@ GPSCoord makeGPSCoord(double lat, double lng, double alt) { return coord; } -std::string ODLCShapeToString(const ODLCShape& shape) { - switch (shape) { - case ODLCShape::Circle: return "CIRCLE"; - case ODLCShape::Semicircle: return "SEMICIRCLE"; - case ODLCShape::QuarterCircle: return "QUARTERCIRCLE"; - case ODLCShape::Triangle: return "TRIANGLE"; - case ODLCShape::Rectangle: return "RECTANGLE"; - case ODLCShape::Pentagon: return "PENTAGON"; - case ODLCShape::Star: return "STAR"; - case ODLCShape::Cross: return "CROSS"; - default: return "CIRCLE"; - } -} -std::string ODLCColorToString(const ODLCColor& color) { +std::string ODLCObjectsToString(const ODLCObjects& color) { switch (color) { - case ODLCColor::White: return "WHITE"; - case ODLCColor::Black: return "BLACK"; - case ODLCColor::Red: return "RED"; - case ODLCColor::Blue: return "BLUE"; - case ODLCColor::Green: return "GREEN"; - case ODLCColor::Purple: return "PURPLE"; - case ODLCColor::Brown: return "BROWN"; - case ODLCColor::Orange: return "ORANGE"; - // maybe return optional nullopt here instead of defaulting to WHITE - // in case of an unknown color - default: return "WHITE"; + case ODLCObjects::Mannequin: return "MANNEQUIN"; + case ODLCObjects::Car: return "CAR"; + case ODLCObjects::Motorcycle: return "MOTORCYCLE"; + case ODLCObjects::Airplane: return "AIRPLANE"; + case ODLCObjects::Bus: return "BUS"; + case ODLCObjects::Boat: return "BOAT"; + case ODLCObjects::Stopsign: return "STOPSIGN"; + case ODLCObjects::Snowboard: return "SNOWBOARD"; + case ODLCObjects::Umbrella: return "UMBRELLA"; + case ODLCObjects::SoccerBall: return "SOCCERBALL"; + case ODLCObjects::Basketball: return "BASKETBALL"; + case ODLCObjects::Volleyball: return "VOLLEYBALL"; + case ODLCObjects::Football: return "FOOTBALL"; + case ODLCObjects::Baseballbat: return "BASEBALLBAT"; + case ODLCObjects::Mattress: return "MATTRESS"; + case ODLCObjects::Tennisracket: return "TENNISRACKET"; + case ODLCObjects::Suitcase: return "SUITCASE"; + case ODLCObjects::Skis: return "SKIS"; + // maybe return optional nullopt here instead of defaulting to IDFK + // in case of an unknown object (Not relevant anymore I don't think) + default: return "IDFK"; } } diff --git a/src/utilities/obc_config.cpp b/src/utilities/obc_config.cpp index 9314d68f..ef08bec5 100644 --- a/src/utilities/obc_config.cpp +++ b/src/utilities/obc_config.cpp @@ -1,9 +1,9 @@ #include "utilities/obc_config.hpp" #include +#include #include #include -#include #include "nlohmann/json.hpp" #include "udp_squared/internal/enum.h" @@ -16,9 +16,15 @@ OBCConfig::OBCConfig(int argc, char* argv[]) { // If config-json name is passed in if (argc != 5) { LOG_F(ERROR, "INVALID COMMAND LINE ARGUMENTS"); - LOG_F(ERROR, "Expected use: ./obcpp [config_dir] [rel_config_path] [plane_name] [flight_type]"); // NOLINT - LOG_F(ERROR, "Example 1 (Test Flight with Jetson): bin/obcpp ../configs jetson stickbug test-flight"); // NOLINT - LOG_F(ERROR, "Example 2 (Local Testing with SITL): bin/obcpp ../configs dev stickbug sitl"); // NOLINT + LOG_F( + ERROR, + "Expected use: ./obcpp [config_dir] [rel_config_path] [plane_name] [flight_type]"); // NOLINT + LOG_F(ERROR, + "Example 1 (Test Flight with Jetson): bin/obcpp ../configs jetson stickbug " + "test-flight"); // NOLINT + LOG_F( + ERROR, + "Example 2 (Local Testing with SITL): bin/obcpp ../configs dev stickbug sitl"); // NOLINT LOG_F(ERROR, "For more help, check the README"); LOG_F(FATAL, "ABORTING..."); } @@ -55,6 +61,7 @@ OBCConfig::OBCConfig(int argc, char* argv[]) { SET_CONFIG_OPT(pathing, rrt, iterations_per_waypoint); SET_CONFIG_OPT(pathing, rrt, rewire_radius); SET_CONFIG_OPT(pathing, rrt, optimize); + SET_CONFIG_OPT(pathing, rrt, generate_deviations); SET_CONFIG_OPT_VARIANT(PointFetchMethod, pathing, rrt, point_fetch_method); @@ -70,9 +77,7 @@ OBCConfig::OBCConfig(int argc, char* argv[]) { SET_CONFIG_OPT(pathing, coverage, forward, vertical); SET_CONFIG_OPT(pathing, coverage, forward, one_way); - SET_CONFIG_OPT(cv, matching_model_dir); - SET_CONFIG_OPT(cv, segmentation_model_dir); - SET_CONFIG_OPT(cv, saliency_model_dir); + SET_CONFIG_OPT(cv, yolo_model_dir); SET_CONFIG_OPT(cv, not_stolen_addr); SET_CONFIG_OPT(cv, not_stolen_port); @@ -91,18 +96,19 @@ OBCConfig::OBCConfig(int argc, char* argv[]) { SET_CONFIG_OPT(camera, mock, images_dir); SET_CONFIG_OPT(takeoff, altitude_m); + SET_CONFIG_OPT(takeoff, payload_size); std::string common_params_path = params_dir / "common.json"; std::ifstream common_params_stream(common_params_path); if (!common_params_stream.is_open()) { LOG_F(FATAL, "Invalid path to common params file: %s (Does this file exist?)", - common_params_path.c_str()); + common_params_path.c_str()); } std::string specific_params_path = params_dir / flight_type_file; std::ifstream specific_params_stream(specific_params_path); if (!specific_params_stream.is_open()) { LOG_F(FATAL, "Invalid path to specific params file: %s (Does this file exist?)", - specific_params_path.c_str()); + specific_params_path.c_str()); } auto common_params = nlohmann::json::parse(common_params_stream, nullptr, true, true); diff --git a/tests/integration/CMakeLists.txt b/tests/integration/CMakeLists.txt index b1f9e6d7..674edbd7 100644 --- a/tests/integration/CMakeLists.txt +++ b/tests/integration/CMakeLists.txt @@ -10,6 +10,7 @@ target_add_mavsdk(playground) target_add_matplot(playground) target_add_protobuf(playground) target_add_loguru(playground) +target_add_onnxruntime(playground) # for some reason calling target_add_imagemagick here conflicts with, so we are including/linking without the function call # target_add_imagemagick(path_plotting) target_include_directories(playground PRIVATE ${ImageMagick_INCLUDE_DIRS}) @@ -35,6 +36,7 @@ target_add_httplib(mavlink_client) target_add_mavsdk(mavlink_client) target_add_matplot(mavlink_client) target_add_loguru(mavlink_client) +target_add_onnxruntime(mavlink_client) # for some reason calling target_add_imagemagick here conflicts with, so we are including/linking without the function call # target_add_imagemagick(path_plotting) target_include_directories(mavlink_client PRIVATE ${ImageMagick_INCLUDE_DIRS}) @@ -53,6 +55,7 @@ target_add_torch(cv_pipeline) target_add_torchvision(cv_pipeline) target_add_mavsdk(cv_pipeline) target_add_loguru(cv_pipeline) +target_add_onnxruntime(cv_pipeline) # for some reason calling target_add_imagemagick here conflicts with, so we are including/linking without the function call # target_add_imagemagick(path_plotting) target_include_directories(cv_pipeline PRIVATE ${ImageMagick_INCLUDE_DIRS}) @@ -71,40 +74,22 @@ target_add_torch(cv_aggregator) target_add_torchvision(cv_aggregator) target_add_mavsdk(cv_aggregator) target_add_loguru(cv_aggregator) +target_add_onnxruntime(cv_aggregator) # for some reason calling target_add_imagemagick here conflicts with, so we are including/linking without the function call # target_add_imagemagick(path_plotting) target_include_directories(cv_aggregator PRIVATE ${ImageMagick_INCLUDE_DIRS}) target_link_libraries(cv_aggregator PRIVATE -Wl,--copy-dt-needed-entries ${ImageMagick_LIBRARIES}) -# cv_matching -add_executable(cv_matching "cv_matching.cpp") -target_link_libraries(cv_matching PRIVATE obcpp_lib) -target_add_json(cv_matching) -target_add_matplot(cv_matching) -target_add_opencv(cv_matching) -target_add_torch(cv_matching) -target_add_torchvision(cv_matching) -target_add_loguru(cv_matching) -target_add_httplib(cv_matching) -target_add_mavsdk(cv_matching) -# for some reason calling target_add_imagemagick here conflicts with, so we are including/linking without the function call -# target_add_imagemagick(path_plotting) -target_include_directories(cv_matching PRIVATE ${ImageMagick_INCLUDE_DIRS}) -target_link_libraries(cv_matching PRIVATE -Wl,--copy-dt-needed-entries ${ImageMagick_LIBRARIES}) - - -# cv_saliency -add_executable(cv_saliency ${SOURCES} "cv_saliency.cpp") -target_link_libraries(cv_saliency PRIVATE obcpp_lib) -target_include_directories(cv_saliency PRIVATE ${INCLUDE_DIRECTORY}) -target_add_torch(cv_saliency) -target_add_opencv(cv_saliency) -target_add_torchvision(cv_saliency) -target_add_matplot(cv_saliency) -target_add_json(cv_saliency) -target_add_loguru(cv_saliency) -target_include_directories(cv_saliency PRIVATE ${ImageMagick_INCLUDE_DIRS}) -target_link_libraries(cv_saliency PRIVATE -Wl,--copy-dt-needed-entries ${ImageMagick_LIBRARIES}) +# cv_preprocess +add_executable(cv_preprocess ${SOURCES} "cv_preprocess.cpp") +target_link_libraries(cv_preprocess PRIVATE obcpp_lib) +target_include_directories(cv_preprocess PRIVATE ${INCLUDE_DIRECTORY}) +target_add_opencv(cv_preprocess) +target_add_matplot(cv_preprocess) +target_add_json(cv_preprocess) +target_add_loguru(cv_preprocess) +target_include_directories(cv_preprocess PRIVATE ${ImageMagick_INCLUDE_DIRS}) +target_link_libraries(cv_preprocess PRIVATE -Wl,--copy-dt-needed-entries ${ImageMagick_LIBRARIES}) # cv_mapping add_executable(cv_mapping ${SOURCES} "cv_mapping.cpp") @@ -117,21 +102,19 @@ target_add_loguru(cv_mapping) target_include_directories(cv_mapping PRIVATE ${ImageMagick_INCLUDE_DIRS}) target_link_libraries(cv_mapping PRIVATE -Wl,--copy-dt-needed-entries ${ImageMagick_LIBRARIES}) -# cv_segmentation -add_executable(cv_segmentation "cv_segmentation.cpp") -target_link_libraries(cv_segmentation PRIVATE obcpp_lib) -target_add_json(cv_segmentation) -target_add_matplot(cv_segmentation) -target_add_opencv(cv_segmentation) -target_add_torch(cv_segmentation) -target_add_torchvision(cv_segmentation) -target_add_loguru(cv_segmentation) -target_add_httplib(cv_segmentation) -target_add_mavsdk(cv_segmentation) -# for some reason calling target_add_imagemagick here conflicts with, so we are including/linking without the function call -# target_add_imagemagick(path_plotting) -target_include_directories(cv_segmentation PRIVATE ${ImageMagick_INCLUDE_DIRS}) -target_link_libraries(cv_segmentation PRIVATE -Wl,--copy-dt-needed-entries ${ImageMagick_LIBRARIES}) +# cv_yolo +add_executable(cv_yolo ${SOURCES} "cv_yolo.cpp") +target_link_libraries(cv_yolo PRIVATE obcpp_lib) +target_include_directories(cv_yolo PRIVATE ${INCLUDE_DIRECTORY}) +target_add_torch(cv_yolo) +target_add_opencv(cv_yolo) +target_add_torchvision(cv_yolo) +target_add_matplot(cv_yolo) +target_add_json(cv_yolo) +target_add_loguru(cv_yolo) +target_add_onnxruntime(cv_yolo) +target_include_directories(cv_yolo PRIVATE ${ImageMagick_INCLUDE_DIRS}) +target_link_libraries(cv_yolo PRIVATE -Wl,--copy-dt-needed-entries ${ImageMagick_LIBRARIES}) # for some reason calling target_add_imagemagick here conflicts with, so we are including/linking without the function call # target_add_imagemagick(${PROJECT_NAME}) @@ -148,6 +131,7 @@ target_add_matplot(path_plotting) target_add_protobuf(path_plotting) target_add_opencv(path_plotting) target_add_loguru(path_plotting) +target_add_onnxruntime(path_plotting) # for some reason calling target_add_imagemagick here conflicts with, so we are including/linking without the function call # target_add_imagemagick(path_plotting) target_include_directories(path_plotting PRIVATE ${ImageMagick_INCLUDE_DIRS}) @@ -165,6 +149,7 @@ target_add_httplib(path_planning) target_add_mavsdk(path_planning) target_add_matplot(path_planning) target_add_loguru(path_planning) +target_add_onnxruntime(path_planning) target_include_directories(path_planning PRIVATE ${ImageMagick_INCLUDE_DIRS}) target_link_libraries(path_planning PRIVATE -Wl,--copy-dt-needed-entries ${ImageMagick_LIBRARIES}) @@ -182,6 +167,7 @@ target_add_httplib(pathing_integration) target_add_mavsdk(pathing_integration) target_add_matplot(pathing_integration) target_add_loguru(pathing_integration) +target_add_onnxruntime(pathing_integration) target_include_directories(pathing_integration PRIVATE ${ImageMagick_INCLUDE_DIRS}) target_link_libraries(pathing_integration PRIVATE -Wl,--copy-dt-needed-entries ${ImageMagick_LIBRARIES}) @@ -198,6 +184,7 @@ target_add_httplib(coverage_pathing) target_add_mavsdk(coverage_pathing) target_add_matplot(coverage_pathing) target_add_loguru(coverage_pathing) +target_add_onnxruntime(coverage_pathing) target_include_directories(coverage_pathing PRIVATE ${ImageMagick_INCLUDE_DIRS}) target_link_libraries(coverage_pathing PRIVATE -Wl,--copy-dt-needed-entries ${ImageMagick_LIBRARIES}) @@ -214,6 +201,7 @@ target_add_httplib(coverage_pathing_2) target_add_mavsdk(coverage_pathing_2) target_add_matplot(coverage_pathing_2) target_add_loguru(coverage_pathing_2) +target_add_onnxruntime(coverage_pathing_2) target_include_directories(coverage_pathing_2 PRIVATE ${ImageMagick_INCLUDE_DIRS}) target_link_libraries(coverage_pathing_2 PRIVATE -Wl,--copy-dt-needed-entries ${ImageMagick_LIBRARIES}) @@ -230,6 +218,7 @@ target_add_httplib(airdrop_approach) target_add_mavsdk(airdrop_approach) target_add_matplot(airdrop_approach) target_add_loguru(airdrop_approach) +target_add_onnxruntime(airdrop_approach) target_include_directories(airdrop_approach PRIVATE ${ImageMagick_INCLUDE_DIRS}) target_link_libraries(airdrop_approach PRIVATE -Wl,--copy-dt-needed-entries ${ImageMagick_LIBRARIES}) @@ -262,6 +251,7 @@ target_add_httplib(airdrop_packets) target_add_mavsdk(airdrop_packets) target_add_matplot(airdrop_packets) target_add_loguru(airdrop_packets) +target_add_onnxruntime(airdrop_packets) target_include_directories(airdrop_packets PRIVATE ${ImageMagick_INCLUDE_DIRS}) target_link_libraries(airdrop_packets PRIVATE -Wl,--copy-dt-needed-entries ${ImageMagick_LIBRARIES}) @@ -290,6 +280,7 @@ target_add_httplib(camera_mock) target_add_mavsdk(camera_mock) target_add_matplot(camera_mock) target_add_loguru(camera_mock) +target_add_onnxruntime(camera_mock) target_include_directories(camera_mock PRIVATE ${ImageMagick_INCLUDE_DIRS}) target_link_libraries(camera_mock PRIVATE -Wl,--copy-dt-needed-entries ${ImageMagick_LIBRARIES}) @@ -303,7 +294,26 @@ target_add_matplot(cuda_check) target_add_protobuf(cuda_check) target_add_opencv(cuda_check) target_add_loguru(cuda_check) +target_add_onnxruntime(cuda_check) # for some reason calling target_add_imagemagick here conflicts with, so we are including/linking without the function call # target_add_imagemagick(cuda_check) target_include_directories(cuda_check PRIVATE ${ImageMagick_INCLUDE_DIRS}) -target_link_libraries(cuda_check PRIVATE -Wl,--copy-dt-needed-entries ${ImageMagick_LIBRARIES}) \ No newline at end of file +target_link_libraries(cuda_check PRIVATE -Wl,--copy-dt-needed-entries ${ImageMagick_LIBRARIES}) + + + +add_executable(drop_it_like_its_hot "drop_it_like_its_hot.cpp") +target_link_libraries(drop_it_like_its_hot PRIVATE obcpp_lib) +target_include_directories(drop_it_like_its_hot PRIVATE ${INCLUDE_DIRECTORY}) +target_add_protobuf(drop_it_like_its_hot) +target_add_torch(drop_it_like_its_hot) +target_add_torchvision(drop_it_like_its_hot) +target_add_json(drop_it_like_its_hot) +target_add_opencv(drop_it_like_its_hot) +target_add_httplib(drop_it_like_its_hot) +target_add_mavsdk(drop_it_like_its_hot) +target_add_matplot(drop_it_like_its_hot) +target_add_loguru(drop_it_like_its_hot) +target_add_onnxruntime(drop_it_like_its_hot) +target_include_directories(drop_it_like_its_hot PRIVATE ${ImageMagick_INCLUDE_DIRS}) +target_link_libraries(drop_it_like_its_hot PRIVATE -Wl,--copy-dt-needed-entries ${ImageMagick_LIBRARIES}) diff --git a/tests/integration/aggregator/.gitkeep b/tests/integration/aggregator/.gitkeep new file mode 100644 index 00000000..e69de29b diff --git a/tests/integration/airdrop_packets.cpp b/tests/integration/airdrop_packets.cpp index daf540eb..eaa567cd 100644 --- a/tests/integration/airdrop_packets.cpp +++ b/tests/integration/airdrop_packets.cpp @@ -27,74 +27,69 @@ int main(int argc, char** argv) { for (int i = 0; i < 16; i++) { std::cout << "DROP A\n"; - airdrop.send(makeDropNowPacket(bottle_t::UDP2_A)); + airdrop.send(makeDropNowPacket(airdrop_t::UDP2_A)); std::this_thread::sleep_for(100ms); } for (int i = 0; i < 16; i++) { std::cout << "DROP B\n"; - airdrop.send(makeDropNowPacket(bottle_t::UDP2_B)); + airdrop.send(makeDropNowPacket(airdrop_t::UDP2_B)); std::this_thread::sleep_for(100ms); } for (int i = 0; i < 16; i++) { std::cout << "DROP C\n"; - airdrop.send(makeDropNowPacket(bottle_t::UDP2_C)); + airdrop.send(makeDropNowPacket(airdrop_t::UDP2_C)); std::this_thread::sleep_for(100ms); } for (int i = 0; i < 16; i++) { std::cout << "DROP D\n"; - airdrop.send(makeDropNowPacket(bottle_t::UDP2_D)); - std::this_thread::sleep_for(100ms); - } - for (int i = 0; i < 16; i++) { - std::cout << "DROP E\n"; - airdrop.send(makeDropNowPacket(bottle_t::UDP2_E)); + airdrop.send(makeDropNowPacket(airdrop_t::UDP2_D)); std::this_thread::sleep_for(100ms); } - // airdrop.send(makeLatLngPacket(SEND_LATLNG, UDP2_A, OBC_NULL, 32.123, 76.321, 100)); - // airdrop.send(makeLatLngPacket(SEND_LATLNG, UDP2_A, OBC_NULL, 32.123, 76.321, 100)); - // airdrop.send(makeLatLngPacket(SEND_LATLNG, UDP2_A, OBC_NULL, 32.123, 76.321, 100)); - // airdrop.send(makeLatLngPacket(SEND_LATLNG, UDP2_A, OBC_NULL, 32.123, 76.321, 100)); - // std::this_thread::sleep_for(3s); - // LOG_F(INFO, "Checking for ack latlng..."); - // while (true) { - // auto packet = airdrop.receive(); - // if (!packet) { - // break; - // } - // uint8_t bottle, state; - // parseID(packet->id, &bottle, &state); - // LOG_F(INFO, "rec ack latlng? %d %d %d", static_cast(packet->header), static_cast(bottle), static_cast(state)); - // } - // LOG_F(INFO, "Done with checking for ack latlng"); - // airdrop.send(makeArmPacket(ARM, UDP2_A, OBC_NULL, 105)); - // airdrop.send(makeArmPacket(ARM, UDP2_A, OBC_NULL, 105)); - // airdrop.send(makeArmPacket(ARM, UDP2_A, OBC_NULL, 105)); - // airdrop.send(makeArmPacket(ARM, UDP2_A, OBC_NULL, 105)); - // std::this_thread::sleep_for(3s); - // while (true) { - // auto packet = airdrop.receive(); - // if (!packet) { - // break; - // } - // uint8_t bottle, state; - // parseID(packet->id, &bottle, &state); - // LOG_F(INFO, "recv ack arm? %d %d %d", static_cast(packet->header), static_cast(bottle), static_cast(state)); - // } - // airdrop.send(makeArmPacket(DISARM, UDP2_A, OBC_NULL, 100)); - // airdrop.send(makeArmPacket(DISARM, UDP2_A, OBC_NULL, 100)); - // airdrop.send(makeArmPacket(DISARM, UDP2_A, OBC_NULL, 100)); - // airdrop.send(makeArmPacket(DISARM, UDP2_A, OBC_NULL, 100)); - // std::this_thread::sleep_for(3s); - // while (true) { - // auto packet = airdrop.receive(); - // if (!packet) { - // break; - // } - // uint8_t bottle, state; - // parseID(packet->id, &bottle, &state); - // LOG_F(INFO, "recv ack disarm? %d %d %d", static_cast(packet->header), static_cast(bottle), static_cast(state)); - // } + airdrop.send(makeLatLngPacket(SEND_LATLNG, UDP2_A, OBC_NULL, 32.123, 76.321, 100)); + airdrop.send(makeLatLngPacket(SEND_LATLNG, UDP2_A, OBC_NULL, 32.123, 76.321, 100)); + airdrop.send(makeLatLngPacket(SEND_LATLNG, UDP2_A, OBC_NULL, 32.123, 76.321, 100)); + airdrop.send(makeLatLngPacket(SEND_LATLNG, UDP2_A, OBC_NULL, 32.123, 76.321, 100)); + std::this_thread::sleep_for(3s); + LOG_F(INFO, "Checking for ack latlng..."); + while (true) { + auto packet = airdrop.receive(); + if (!packet) { + break; + } + uint8_t airdrop, state; + parseID(packet->id, &airdrop, &state); + LOG_F(INFO, "rec ack latlng? %d %d %d", static_cast(packet->header), static_cast(airdrop), static_cast(state)); + } + LOG_F(INFO, "Done with checking for ack latlng"); + airdrop.send(makeArmPacket(ARM, UDP2_A, OBC_NULL, 105)); + airdrop.send(makeArmPacket(ARM, UDP2_A, OBC_NULL, 105)); + airdrop.send(makeArmPacket(ARM, UDP2_A, OBC_NULL, 105)); + airdrop.send(makeArmPacket(ARM, UDP2_A, OBC_NULL, 105)); + std::this_thread::sleep_for(3s); + while (true) { + auto packet = airdrop.receive(); + if (!packet) { + break; + } + uint8_t airdrop, state; + parseID(packet->id, &airdrop, &state); + LOG_F(INFO, "recv ack arm? %d %d %d", static_cast(packet->header), static_cast(airdrop), static_cast(state)); + } + airdrop.send(makeArmPacket(DISARM, UDP2_A, OBC_NULL, 100)); + airdrop.send(makeArmPacket(DISARM, UDP2_A, OBC_NULL, 100)); + airdrop.send(makeArmPacket(DISARM, UDP2_A, OBC_NULL, 100)); + airdrop.send(makeArmPacket(DISARM, UDP2_A, OBC_NULL, 100)); + std::this_thread::sleep_for(3s); + while (true) { + auto packet = airdrop.receive(); + if (!packet) { + break; + } + uint8_t airdrop, state; + parseID(packet->id, &airdrop, &state); + LOG_F(INFO, "recv ack disarm? %d %d %d", static_cast(packet->header), static_cast(airdrop), static_cast(state)); + } while (true) { std::cout << "stop\n"; diff --git a/tests/integration/cv_aggregator.cpp b/tests/integration/cv_aggregator.cpp index 29a3d43b..64bd7133 100644 --- a/tests/integration/cv_aggregator.cpp +++ b/tests/integration/cv_aggregator.cpp @@ -1,112 +1,92 @@ +#include +#include #include - -#include #include +#include +#include +#include -#include "cv/pipeline.hpp" #include "cv/aggregator.hpp" +#include "cv/pipeline.hpp" + +namespace fs = std::filesystem; -// Download these test images from one of the zips here https://drive.google.com/drive/u/1/folders/1opXBWx6bBF7J3-JSFtrhfFIkYis9qhGR -// Or, any cropped not-stolen images will work - -// this image should be located at a relative path to the CMake build dir -const std::string imageTestDir = "../tests/integration/images/matching_cropped/test/"; -const std::string refImagePath0 = imageTestDir + "000000910.jpg"; // bottle 4 -const std::string refImagePath1 = imageTestDir + "000000920.jpg"; // bottle 3 -const std::string refImagePath2 = imageTestDir + "000000003.jpg"; // bottle 2 -const std::string refImagePath3 = imageTestDir + "000000004.jpg"; // bottle 1 -const std::string refImagePath4 = imageTestDir + "000000005.jpg"; // bottle 0 - -// matching model can be downloaded from here: https://drive.google.com/drive/folders/1ciDfycNyJiLvRhJhwQZoeKH7vgV6dGHJ?usp=drive_link -const std::string matchingModelPath = "../models/target_siamese_1.pt"; -// segmentation model can be downloaded from here: https://drive.google.com/file/d/1U2EbfJFzcjVnjTuD6ud-bIf8YOiEassf/view?usp=drive_link -const std::string segmentationModelPath = "../models/fcn.pth"; - -// mock telemetry data -double latitude = 38.31568; -double longitude = 76.55006; -double altitude = 75; -double airspeed = 20; -double yaw = 100; -double pitch = 5; -double roll = 3; - -// integration test to test all stages of the CV pipeline -// with an arbitrary image as input int main() { - cv::Mat image = cv::imread(imageTestDir + "000000008.jpg"); - assert(!image.empty()); - ImageTelemetry mockTelemetry(latitude, longitude, altitude, airspeed, - yaw, pitch, roll); - ImageData imageData("000000008.jpg", imageTestDir, image, mockTelemetry); - - std::array bottlesToDrop; - - Bottle bottle1; - bottle1.set_shapecolor(ODLCColor::Red); - bottle1.set_shape(ODLCShape::Circle); - bottle1.set_alphanumericcolor(ODLCColor::Orange); - bottle1.set_alphanumeric("J"); - bottlesToDrop[0] = bottle1; - - Bottle bottle2; - bottle2.set_shapecolor(ODLCColor::Blue); - bottle2.set_shape(ODLCShape::Circle); - bottle2.set_alphanumericcolor(ODLCColor::Orange); - bottle2.set_alphanumeric("G"); - bottlesToDrop[1] = bottle2; - - Bottle bottle3; - bottle3.set_shapecolor(ODLCColor::Red); - bottle3.set_shape(ODLCShape::Circle); - bottle3.set_alphanumericcolor(ODLCColor::Blue); - bottle3.set_alphanumeric("X"); - bottlesToDrop[2] = bottle3; - - Bottle bottle4; - bottle4.set_shapecolor(ODLCColor::Red); - bottle4.set_shape(ODLCShape::Circle); - bottle4.set_alphanumericcolor(ODLCColor::Blue); - bottle4.set_alphanumeric("F"); - bottlesToDrop[3] = bottle4; - - Bottle bottle5; - bottle5.set_shapecolor(ODLCColor::Green); - bottle5.set_shape(ODLCShape::Circle); - bottle5.set_alphanumericcolor(ODLCColor::Black); - bottle5.set_alphanumeric("F"); - bottlesToDrop[4] = bottle5; - - std::vector> referenceImages; - cv::Mat ref0 = cv::imread(refImagePath0); - referenceImages.push_back(std::make_pair(ref0, BottleDropIndex(5))); - cv::Mat ref1 = cv::imread(refImagePath1); - referenceImages.push_back(std::make_pair(ref1, BottleDropIndex(4))); - cv::Mat ref2 = cv::imread(refImagePath2); - referenceImages.push_back(std::make_pair(ref2, BottleDropIndex(3))); - cv::Mat ref3 = cv::imread(refImagePath3); - referenceImages.push_back(std::make_pair(ref3, BottleDropIndex(2))); - cv::Mat ref4 = cv::imread(refImagePath4); - referenceImages.push_back(std::make_pair(ref4, BottleDropIndex(1))); - - Pipeline pipeline(PipelineParams(bottlesToDrop, referenceImages, matchingModelPath, segmentationModelPath)); - - // Same setup as cv_pipeline unit test up until this point - - std::cout << "about to create aggregator\n"; - - CVAggregator aggregator(pipeline); - - std::cout << "about to run aggregator\n"; - - aggregator.runPipeline(imageData); - aggregator.runPipeline(imageData); - aggregator.runPipeline(imageData); - aggregator.runPipeline(imageData); - aggregator.runPipeline(imageData); - aggregator.runPipeline(imageData); - aggregator.runPipeline(imageData); - aggregator.runPipeline(imageData); - - sleep(100000); -} \ No newline at end of file + // Define the directory containing the test images + std::string imagesDirectory = "../tests/integration/aggregator/"; + std::vector imagePaths; + + // Iterate over the directory to collect all .jpg, .jpeg, and .png files + for (const auto& entry : fs::directory_iterator(imagesDirectory)) { + if (entry.is_regular_file()) { + std::string ext = entry.path().extension().string(); + if (ext == ".jpg" || ext == ".jpeg" || ext == ".png") { + imagePaths.push_back(entry.path().string()); + } + } + } + + if (imagePaths.empty()) { + std::cerr << "No image files found in directory: " << imagesDirectory << std::endl; + return 1; + } + + // Construct the pipeline with the desired YOLO model + PipelineParams params("../models/yolo11x.onnx", + "../tests/integration/output/output_aggregator.jpg", + false); // do_preprocess=false + + Pipeline pipeline(params); + + // Build the aggregator by transferring ownership of the pipeline + CVAggregator aggregator(std::move(pipeline)); + + // Create a mock telemetry object (using the same telemetry for simplicity) + ImageTelemetry mockTelemetry{38.31568, 76.55006, 75, 20, 100, 5, 3}; + + // Submit each image from the directory to the aggregator concurrently + for (const auto& imagePath : imagePaths) { + cv::Mat image = cv::imread(imagePath); + if (image.empty()) { + std::cerr << "Could not open image: " << imagePath << std::endl; + continue; + } + // Create an ImageData instance for each image + ImageData imageData(image, 0, mockTelemetry); + aggregator.runPipeline(imageData); + } + + // Let the worker threads finish + std::this_thread::sleep_for(std::chrono::seconds(5)); + + // Retrieve the aggregated results in a thread-safe manner + auto lockedResults = aggregator.getResults(); + auto resultsPtr = lockedResults.data; + if (!resultsPtr) { + std::cerr << "Error: aggregator returned null results?" << std::endl; + return 1; + } + + // Count total detections across all runs + size_t totalDetections = 0; + for (const auto& run : resultsPtr->runs) { + totalDetections += run.bboxes.size(); + } + + // Print out bounding box info for each run + for (size_t runIdx = 0; runIdx < resultsPtr->runs.size(); ++runIdx) { + const auto& run = resultsPtr->runs[runIdx]; + std::cout << "=== AggregatedRun #" << runIdx << " ===" << std::endl; + + for (size_t detIdx = 0; detIdx < run.bboxes.size(); ++detIdx) { + const auto& bbox = run.bboxes[detIdx]; + const auto& coord = run.coords[detIdx]; + + std::cout << " Detection #" << detIdx << " BBox=[" << bbox.x1 << "," << bbox.y1 << "," + << bbox.x2 << "," << bbox.y2 << "]" + << " Lat=" << coord.latitude() << " Lon=" << coord.longitude() << std::endl; + } + } + + return 0; +} diff --git a/tests/integration/cv_mapping.cpp b/tests/integration/cv_mapping.cpp index a676ef70..f15c10e5 100644 --- a/tests/integration/cv_mapping.cpp +++ b/tests/integration/cv_mapping.cpp @@ -25,7 +25,6 @@ int main() { // 1. Simulate receiving Batch 1 images // Clear out the live_images_dir if you want to start fresh - // (Alternatively, you can let it remain if there's something from a prior run.) for (const auto& entry : fs::directory_iterator(live_images_dir)) { fs::remove_all(entry.path()); } @@ -44,8 +43,9 @@ int main() { const int max_dim = 3000; std::cout << "==> Processing Batch 1...\n"; + // Note: the new last argument 'preprocess' defaults to true. mapper.firstPass(live_images_dir.string(), output_dir.string(), chunk_size, chunk_overlap, - scan_mode, max_dim); + scan_mode, max_dim, true); // 3. Simulate time passing, and receiving Batch 2 images // Copy Batch 2 images into "images/" @@ -59,16 +59,103 @@ int main() { // 4. Process the second batch with the same mapper instance std::cout << "==> Processing Batch 2...\n"; mapper.firstPass(live_images_dir.string(), output_dir.string(), chunk_size, chunk_overlap, - scan_mode, max_dim); + scan_mode, max_dim, true); - // 5. Finally, call secondPass to merge all chunk images in output/ + // 5. Finally, call secondPass to merge all chunk images in the timestamped run folder std::cout << "==> Merging all partial chunk images into final...\n"; - mapper.secondPass(output_dir.string(), scan_mode, max_dim); + mapper.secondPass(output_dir.string(), scan_mode, max_dim, true); std::cout << "Integration test completed.\n"; } catch (const std::exception& e) { std::cerr << "Exception in incremental integration test: " << e.what() << "\n"; return 1; } + + // ========================================================================= + // Test 2: Direct Stitch Integration Test + // ========================================================================= + std::cout << "\n" << std::string(60, '=') << "\n"; + std::cout << "Starting Direct Stitch Integration Test\n"; + std::cout << std::string(60, '=') << "\n"; + + try { + // Set up paths for direct stitch test + fs::path base_dir = "../tests/integration/mapping"; + fs::path batch1_dir = base_dir / "batch1"; + fs::path batch2_dir = base_dir / "batch2"; + fs::path direct_test_dir = base_dir / "direct_test_images"; + fs::path direct_output_dir = base_dir / "direct_output"; + + // Create test directories + if (!fs::exists(direct_test_dir)) { + fs::create_directories(direct_test_dir); + } + if (!fs::exists(direct_output_dir)) { + fs::create_directories(direct_output_dir); + } + + // Clear the direct test directory + for (const auto& entry : fs::directory_iterator(direct_test_dir)) { + fs::remove_all(entry.path()); + } + + // Copy ALL images from both batches into the direct test directory + std::cout << "Copying images from batch1 and batch2 to direct test directory...\n"; + int image_count = 0; + + // Copy from batch1 + if (fs::exists(batch1_dir)) { + for (const auto& entry : fs::directory_iterator(batch1_dir)) { + if (entry.is_regular_file()) { + fs::copy_file(entry.path(), direct_test_dir / entry.path().filename(), + fs::copy_options::overwrite_existing); + image_count++; + } + } + } + + // Copy from batch2 + if (fs::exists(batch2_dir)) { + for (const auto& entry : fs::directory_iterator(batch2_dir)) { + if (entry.is_regular_file()) { + fs::copy_file(entry.path(), direct_test_dir / entry.path().filename(), + fs::copy_options::overwrite_existing); + image_count++; + } + } + } + + std::cout << "Copied " << image_count << " images for direct stitching test.\n"; + + if (image_count == 0) { + std::cout << "No images found in batch directories. Creating a simple test...\n"; + std::cout << "Please add some test images to batch1/ or batch2/ directories.\n"; + } else { + // Test the direct stitch method + Mapping direct_mapper; + const int max_dim = 3000; + + std::cout << "==> Testing directStitch with preprocessing enabled...\n"; + direct_mapper.directStitch(direct_test_dir.string(), direct_output_dir.string(), + scan_mode, max_dim, true); + + std::cout << "==> Testing directStitch with preprocessing disabled...\n"; + direct_mapper.directStitch(direct_test_dir.string(), + (direct_output_dir / "no_preprocess.jpg").string(), + scan_mode, max_dim, false); + + std::cout << "Direct stitch integration test completed.\n"; + std::cout << "Check the following directory for results: " << direct_output_dir << "\n"; + } + + } catch (const std::exception& e) { + std::cerr << "Exception in direct stitch integration test: " << e.what() << "\n"; + return 1; + } + + std::cout << "\n" << std::string(60, '=') << "\n"; + std::cout << "All integration tests completed successfully!\n"; + std::cout << std::string(60, '=') << "\n"; + return 0; } diff --git a/tests/integration/cv_matching.cpp b/tests/integration/cv_matching.cpp deleted file mode 100644 index 95501366..00000000 --- a/tests/integration/cv_matching.cpp +++ /dev/null @@ -1,119 +0,0 @@ -#include - -#include -#include - -#include "protos/obc.pb.h" - -#include "cv/matching.hpp" -#include "utilities/constants.hpp" -#include "utilities/common.hpp" - -// Download these test images by running "make pull_matching_test_images" or from the test.zip here https://drive.google.com/drive/u/1/folders/1opXBWx6bBF7J3-JSFtrhfFIkYis9qhGR -// Or, any cropped not-stolen images will work -// NOTE: images are given reverse order bottleIndexes, e.g. refImagePath0 -> index 4, etc. -const std::string imageTestDir = "../tests/integration/images/matching_cropped/test/"; -const std::string refImagePath0 = imageTestDir + "000000910.jpg"; // bottle 4 -const std::string refImagePath1 = imageTestDir + "000000920.jpg"; // bottle 3 -const std::string refImagePath2 = imageTestDir + "000000003.jpg"; // bottle 2 -const std::string refImagePath3 = imageTestDir + "000000004.jpg"; // bottle 1 -const std::string refImagePath4 = imageTestDir + "000000005.jpg"; // bottle 0 - -// model can be downloaded by running "make pull_matching" or from here: https://drive.google.com/drive/folders/1ciDfycNyJiLvRhJhwQZoeKH7vgV6dGHJ?usp=drive_link -const std::string modelPath = "../models/target_siamese_1.pt"; - -// These images can also come from the same source as the reference images. To accurately -// run this test, provide one image that is a positive match and one that doesn't match -// any of the references. -const std::string imageMatchPath = imageTestDir + "000000920.jpg"; -const int matchIndex = 3; -const std::string imageNotMatchPath = imageTestDir + "000000016.jpg"; - -int main(int argc, char* argv[]) { - // purely for the constructor, doesn't do much in matching - std::array bottlesToDrop; - - Bottle bottle1; - bottle1.set_shapecolor(ODLCColor::Red); - bottle1.set_shape(ODLCShape::Circle); - bottle1.set_alphanumericcolor(ODLCColor::Orange); - bottle1.set_alphanumeric("J"); - bottlesToDrop[0] = bottle1; - - Bottle bottle2; - bottle2.set_shapecolor(ODLCColor::Blue); - bottle2.set_shape(ODLCShape::Circle); - bottle2.set_alphanumericcolor(ODLCColor::Orange); - bottle2.set_alphanumeric("G"); - bottlesToDrop[1] = bottle2; - - Bottle bottle3; - bottle3.set_shapecolor(ODLCColor::Red); - bottle3.set_shape(ODLCShape::Circle); - bottle3.set_alphanumericcolor(ODLCColor::Blue); - bottle3.set_alphanumeric("X"); - bottlesToDrop[2] = bottle3; - - Bottle bottle4; - bottle4.set_shapecolor(ODLCColor::Red); - bottle4.set_shape(ODLCShape::Circle); - bottle4.set_alphanumericcolor(ODLCColor::Blue); - bottle4.set_alphanumeric("F"); - bottlesToDrop[3] = bottle4; - - Bottle bottle5; - bottle5.set_shapecolor(ODLCColor::Green); - bottle5.set_shape(ODLCShape::Circle); - bottle5.set_alphanumericcolor(ODLCColor::Black); - bottle5.set_alphanumeric("F"); - bottlesToDrop[4] = bottle5; - - std::vector> referenceImages; - cv::Mat ref0 = cv::imread(refImagePath0); - referenceImages.push_back(std::make_pair(ref0, BottleDropIndex(5))); - cv::Mat ref1 = cv::imread(refImagePath1); - referenceImages.push_back(std::make_pair(ref1, BottleDropIndex(4))); - cv::Mat ref2 = cv::imread(refImagePath2); - referenceImages.push_back(std::make_pair(ref2, BottleDropIndex(3))); - cv::Mat ref3 = cv::imread(refImagePath3); - referenceImages.push_back(std::make_pair(ref3, BottleDropIndex(2))); - cv::Mat ref4 = cv::imread(refImagePath4); - referenceImages.push_back(std::make_pair(ref4, BottleDropIndex(1))); - - auto startLoad = getUnixTime_ms().count(); - Matching matcher(bottlesToDrop, referenceImages, modelPath); - auto endLoad = getUnixTime_ms().count(); - LOG_F(INFO, "time to load: %lu milliseconds", endLoad - startLoad); - cv::Mat image = cv::imread(imageMatchPath); - Bbox dummyBox(0, 0, 0, 0); - CroppedTarget cropped = { - image, - dummyBox, - false - }; - - cv::Mat falseImage = cv::imread(imageNotMatchPath); - CroppedTarget croppedFalse = { - falseImage, - dummyBox, - false - }; - - auto startMatch = getUnixTime_ms().count(); - MatchResult result = matcher.match(cropped); - auto endMatch = getUnixTime_ms().count(); - LOG_F(INFO, "\nTRUE MATCH TEST:\nClosest is bottle at index %d\nThe similarity is %.3f\n", - int(result.bottleDropIndex), - result.distance); - - auto startMatchFalse = getUnixTime_ms().count(); - MatchResult resultFalse = matcher.match(croppedFalse); - auto endMatchFalse = getUnixTime_ms().count(); - LOG_F(INFO, "\nFALSE MATCH TEST:\nClosest is bottle at index %d\nThe similarity is %.3f\n", - int(resultFalse.bottleDropIndex), - resultFalse.distance); - LOG_F(INFO, "time to match: %lu milliseconds", - (endMatch - startMatch + endMatchFalse - startMatchFalse)/2); - - return 0; -} \ No newline at end of file diff --git a/tests/integration/cv_pipeline.cpp b/tests/integration/cv_pipeline.cpp index 701f5c95..26856e08 100644 --- a/tests/integration/cv_pipeline.cpp +++ b/tests/integration/cv_pipeline.cpp @@ -1,58 +1,10 @@ #include - -#include #include +#include #include "cv/pipeline.hpp" - -// NOTE: all paths to images should be located at a relative path to the CMake build dir. -// You can download all images with `ninja pull_test_images` - -// Download by running `ninja pull_saliency_test_images` or from this drive folder -// https://drive.google.com/drive/folders/1JvtQUroZJHo51E37_IA2D1mfdJj2smyR?usp=drive_link -const std::string imagePath = "../tests/integration/images/saliency/1718243323.jpg"; - -// Download these test images from here https://drive.google.com/drive/folders/1vmP3HUS1SyqhdtJrP4QuFpGbyoyfaYSe?usp=drive_link -// Or, any cropped not-stolen images will work - -#define NUM_MATCHING_IMAGES 20 -// Directories to matching images. Note that each element of this array is a std::pair. -// The first member is the path to the image and the second member is the bottle each matching -// image corresponds to. See the body of main() for what shape/char/color each bottle is assigned to. -const std::array, NUM_MATCHING_IMAGES> matchingImgPaths = { - std::make_pair("../tests/integration/images/matching/green_rect_purple_B_1.jpg", BottleDropIndex::B), - std::make_pair("../tests/integration/images/matching/green_rect_purple_B_2.jpg", BottleDropIndex::B), - std::make_pair("../tests/integration/images/matching/green_rect_purple_B_3.jpg", BottleDropIndex::B), - std::make_pair("../tests/integration/images/matching/green_rect_purple_B_4.jpg", BottleDropIndex::B), - std::make_pair("../tests/integration/images/matching/green_rect_purple_B_5.jpg", BottleDropIndex::B), - - std::make_pair("../tests/integration/images/matching/red_triangle_purple_E_1.jpg", BottleDropIndex::C), - std::make_pair("../tests/integration/images/matching/red_triangle_purple_E_2.jpg", BottleDropIndex::C), - std::make_pair("../tests/integration/images/matching/red_triangle_purple_E_3.jpg", BottleDropIndex::C), - std::make_pair("../tests/integration/images/matching/red_triangle_purple_E_4.jpg", BottleDropIndex::C), - std::make_pair("../tests/integration/images/matching/red_triangle_purple_E_5.jpg", BottleDropIndex::C), - - std::make_pair("../tests/integration/images/matching/blue_pent_green_Q_1.jpg", BottleDropIndex::D), - std::make_pair("../tests/integration/images/matching/blue_pent_green_Q_2.jpg", BottleDropIndex::D), - std::make_pair("../tests/integration/images/matching/blue_pent_green_Q_3.jpg", BottleDropIndex::D), - std::make_pair("../tests/integration/images/matching/blue_pent_green_Q_4.jpg", BottleDropIndex::D), - std::make_pair("../tests/integration/images/matching/blue_pent_green_Q_5.jpg", BottleDropIndex::D), - - std::make_pair("../tests/integration/images/matching/blue_pent_orange_3_1.jpg", BottleDropIndex::E), - std::make_pair("../tests/integration/images/matching/blue_pent_orange_3_2.jpg", BottleDropIndex::E), - std::make_pair("../tests/integration/images/matching/blue_pent_orange_3_3.jpg", BottleDropIndex::E), - std::make_pair("../tests/integration/images/matching/blue_pent_orange_3_4.jpg", BottleDropIndex::E), - std::make_pair("../tests/integration/images/matching/blue_pent_orange_3_5.jpg", BottleDropIndex::E), -}; - -// matching model can be downloaded from here: https://drive.google.com/drive/folders/1ciDfycNyJiLvRhJhwQZoeKH7vgV6dGHJ?usp=drive_link -const std::string matchingModelPath = "../models/target_siamese_1.pt"; -// segmentation model can be downloaded from here: https://drive.google.com/file/d/1U2EbfJFzcjVnjTuD6ud-bIf8YOiEassf/view?usp=drive_link -const std::string segmentationModelPath = "../models/fcn-model_20-epochs_06-01-2023T21-16-02.pth"; -const std::string saliencyModelPath = "../models/torchscript_19.pth"; - -// mock telemetry data +// Mock telemetry data const double latitude = 32.990795399999996; const double longitude = -117.1282463; const double altitude = 30.108001708984375; @@ -61,73 +13,63 @@ const double yaw = 100; const double pitch = 5; const double roll = 3; -// integration test to test all stages of the CV pipeline -// with an arbitrary image as input -int main() { +int main(int argc, char** argv) { + std::string yoloModelPath = "../models/yolo11x.onnx"; + std::string imagePath = "../tests/integration/images/image3.jpg"; + std::string outputPath = "../tests/integration/output/output_pipeline.jpg"; + + bool do_preprocess = true; // Default: perform preprocessing + + if (argc > 1) { + yoloModelPath = argv[1]; + } + if (argc > 2) { + imagePath = argv[2]; + } + if (argc > 3) { + outputPath = argv[3]; + } + // Optional fourth argument to disable preprocessing (e.g., "false" or "0") + if (argc > 4) { + std::string preprocessArg = argv[4]; + if (preprocessArg == "false" || preprocessArg == "0") { + do_preprocess = false; + } + } + + // Load test image cv::Mat image = cv::imread(imagePath); if (!image.data) { - std::cout << "failed to open testing image from " << imagePath << std::endl; + LOG_F(ERROR, "Failed to open testing image from %s", imagePath.c_str()); return 1; } - ImageTelemetry mockTelemetry(latitude, longitude, altitude, airspeed, - yaw, pitch, roll); - ImageData imageData(image, 0, mockTelemetry); - - std::array bottlesToDrop; - - // Bottle A - Bottle bottle1; - bottle1.set_ismannikin(true); - bottlesToDrop[0] = bottle1; - - // Bottle B - Bottle bottle2; - bottle2.set_shapecolor(ODLCColor::Green); - bottle2.set_shape(ODLCShape::Rectangle); - bottle2.set_alphanumericcolor(ODLCColor::Purple); - bottle2.set_alphanumeric("B"); - bottlesToDrop[1] = bottle2; - - // Bottle C - Bottle bottle3; - bottle3.set_shapecolor(ODLCColor::Red); - bottle3.set_shape(ODLCShape::Triangle); - bottle3.set_alphanumericcolor(ODLCColor::Purple); - bottle3.set_alphanumeric("E"); - bottlesToDrop[2] = bottle3; - // Bottle D - Bottle bottle4; - bottle4.set_shapecolor(ODLCColor::Blue); - bottle4.set_shape(ODLCShape::Pentagon); - bottle4.set_alphanumericcolor(ODLCColor::Green); - bottle4.set_alphanumeric("Q"); - bottlesToDrop[3] = bottle4; - - // Bottle E - Bottle bottle5; - bottle5.set_shapecolor(ODLCColor::Blue); - bottle5.set_shape(ODLCShape::Pentagon); - bottle5.set_alphanumericcolor(ODLCColor::Orange); - bottle5.set_alphanumeric("3"); - bottlesToDrop[4] = bottle5; - - std::vector> referenceImages; - for (const auto& [matchingImgPath, assignedBottle]: matchingImgPaths) { - cv::Mat refImg = cv::imread(matchingImgPath); - referenceImages.push_back(std::make_pair(refImg, assignedBottle)); - } + // Prepare telemetry data + ImageTelemetry mockTelemetry(latitude, longitude, altitude, airspeed, yaw, pitch, roll); + ImageData imageData(image, 0, mockTelemetry); - Pipeline pipeline(PipelineParams(bottlesToDrop, referenceImages, matchingModelPath, segmentationModelPath, saliencyModelPath)); + // Construct the pipeline with the given parameters. + PipelineParams params(yoloModelPath, outputPath, do_preprocess); + Pipeline pipeline(params); + // Run pipeline PipelineResults output = pipeline.run(imageData); + // Report number of detected targets. size_t numTargets = output.targets.size(); - - LOG_F(INFO, "Detected %ld targets", numTargets); - - for (DetectedTarget& t: output.targets) { - LOG_F(INFO, "Detected Bottle %d at (%f %f) with match distance %f \n", - t.likely_bottle, t.coord.latitude(), t.coord.longitude(), t.match_distance); + LOG_F(INFO, "Detected %ld targets.", numTargets); + + // Print details for each detected target. + for (size_t i = 0; i < output.targets.size(); ++i) { + const auto& t = output.targets[i]; + LOG_F(INFO, + "Target #%ld: class_id=%d, match_distance=%.2f, " + "bbox=[%d %d %d %d], lat=%.5f, lon=%.5f", + i, + static_cast(t.likely_airdrop), // YOLO class index + t.match_distance, t.bbox.x1, t.bbox.y1, t.bbox.x2, t.bbox.y2, t.coord.latitude(), + t.coord.longitude()); } -} \ No newline at end of file + + return 0; +} diff --git a/tests/integration/cv_preprocess.cpp b/tests/integration/cv_preprocess.cpp new file mode 100644 index 00000000..560f9466 --- /dev/null +++ b/tests/integration/cv_preprocess.cpp @@ -0,0 +1,39 @@ +#include +#include + +#include "cv/preprocess.hpp" + +int main(int argc, char* argv[]) { + // Set default file paths. + std::string input_image_path = "../tests/integration/images/output_0819.png"; + std::string output_image_path = "../tests/integration/output/preprocess_output.png"; + + // Allow file paths to be overridden by command-line arguments. + if (argc > 1) { + input_image_path = argv[1]; + } + if (argc > 2) { + output_image_path = argv[2]; + } + + // Load the image from disk. + cv::Mat image = cv::imread(input_image_path); + if (image.empty()) { + std::cerr << "Error: Failed to load image: " << input_image_path << std::endl; + return 1; + } + + // Instantiate the Preprocess object and crop the image. + Preprocess preprocess; + cv::Mat cropped_image = preprocess.cropRight(image); + + // Save the cropped image so you can inspect it manually. + if (!cv::imwrite(output_image_path, cropped_image)) { + std::cerr << "Error: Failed to write image to: " << output_image_path << std::endl; + return 1; + } + + std::cout << "Image processed successfully. Cropped image saved to: " << output_image_path + << std::endl; + return 0; +} diff --git a/tests/integration/cv_yolo.cpp b/tests/integration/cv_yolo.cpp new file mode 100644 index 00000000..bbc2ff52 --- /dev/null +++ b/tests/integration/cv_yolo.cpp @@ -0,0 +1,37 @@ +#include + +#include "cv/yolo.hpp" + +int main(int argc, char** argv) { + // Default file paths and parameters + std::string modelPath = "../models/yolo11x.onnx"; + std::string imagePath = "../tests/integration/images/image3.jpg"; + std::string outputPath = "../tests/integration/output/output_yolo.jpg"; + float confThreshold = 0.05f; + + if (argc > 1) { + modelPath = argv[1]; + } + if (argc > 2) { + imagePath = argv[2]; + } + if (argc > 3) { + confThreshold = std::stof(argv[3]); + } + if (argc > 4) { + outputPath = argv[4]; + } + + // Load an image using OpenCV + cv::Mat image = cv::imread(imagePath); + if (image.empty()) { + std::cerr << "Error: Could not open or find the image at " << imagePath << std::endl; + return -1; + } + + // Create the YOLO object and process the image + YOLO yolo(modelPath, confThreshold, 640, 640); + yolo.processAndSaveImage(image, outputPath); + + return 0; +} diff --git a/tests/integration/drop_it_like_its_hot.cpp b/tests/integration/drop_it_like_its_hot.cpp new file mode 100644 index 00000000..03b2a20c --- /dev/null +++ b/tests/integration/drop_it_like_its_hot.cpp @@ -0,0 +1,36 @@ +#include "network/mavlink.hpp" +#include "ticks/airdrop_approach.hpp" +#include "utilities/logging.hpp" + +/** + * To run, call `bin/drop_it_like_its_hot [CONFIG]`. + * + * DROP IT LIKE ITS HOT is the test for airdrop signal. For the 2025 + * competition, our airdrop mechanism is a electro-pelectro-permanent + * magnet that is triggered by the pixhawk. Therefore, we send a signal to the + * Pixhawk to drop the payload. + * + * This is to test if the signal is sent successfully (we should set up the + * apparatus and see if the epm toggles). + * + * For detailed setup, go look at `mavlink_client.cpp`. +*/ +int main(int argc, char* argv[]) { + if (argc != 5) { + LOG_F(ERROR, "Expected use: bin/mavsdk [config]"); + return 1; + } + + // connection to mav + std::shared_ptr mav_ptr(new MavlinkClient(OBCConfig(argc, argv))); + + // dummy info + std::optional next_airdrop_to_drop; + AirdropIndex next_airdrop = static_cast(2); + next_airdrop_to_drop = static_cast(next_airdrop); + + // drop + triggerAirdrop(mav_ptr, next_airdrop_to_drop.value()); + + return 0; +} diff --git a/tests/integration/mapping/direct_output/.gitkeep b/tests/integration/mapping/direct_output/.gitkeep new file mode 100644 index 00000000..e69de29b diff --git a/tests/integration/mapping/direct_test_images/.gitkeep b/tests/integration/mapping/direct_test_images/.gitkeep new file mode 100644 index 00000000..e69de29b diff --git a/tests/integration/mavlink_client.cpp b/tests/integration/mavlink_client.cpp index 73d5fbe9..e98dec73 100644 --- a/tests/integration/mavlink_client.cpp +++ b/tests/integration/mavlink_client.cpp @@ -3,11 +3,15 @@ /* * Test mavsdk connection to an endpoint - * bin/mavsdk [connection_link] - * connection_link -> [protocol]://[ip]:[port] - * example: - * bin/mavlink_client tcp://127.0.0.1:5760 - * bin/mavlink_client serial:///dev/ttyACM0 + * bin/mavsdk [config] + * example config: + * ``` + * "network": { + * "mavlink": { + * "connect": "serial:///dev/ttyACM0", + * OR + * "connect": "tcp://localhost:14552", + * ``` * * Note: if you are trying to test connection with serial from inside the dev container, * you have to follow these steps: @@ -22,14 +26,14 @@ */ int main(int argc, char *argv[]) { - if (argc != 2) { - LOG_F(ERROR, "Expected use: bin/mavsdk [connection]"); + if (argc != 5) { + LOG_F(ERROR, "Expected use: bin/mavsdk [config]"); return 1; } - LOG_S(INFO) << "Attempting to connect at " << argv[1]; + // LOG_S(INFO) << "Attempting to connect at " << argv[1]; - MavlinkClient mav(argv[1]); + MavlinkClient mav(OBCConfig(argc, argv)); while (true) { LOG_S(INFO) << "Groundspeed: " << mav.groundspeed_m_s(); diff --git a/tests/unit/CMakeLists.txt b/tests/unit/CMakeLists.txt index 3e615f62..b2c84d04 100644 --- a/tests/unit/CMakeLists.txt +++ b/tests/unit/CMakeLists.txt @@ -21,6 +21,7 @@ target_add_opencv(${BINARY}) target_add_matplot(${BINARY}) target_add_httplib(${BINARY}) target_add_loguru(${BINARY}) +target_add_onnxruntime(${BINARY}) # for some reason calling target_add_imagemagick here conflicts with, so we are including/linking without the function call # target_add_imagemagick(path_plotting) target_include_directories(${BINARY} PRIVATE ${ImageMagick_INCLUDE_DIRS}) diff --git a/tests/unit/airdrop_client/airdrop_client_test.cpp b/tests/unit/airdrop_client/airdrop_client_test.cpp index a6cf04d2..a388618d 100644 --- a/tests/unit/airdrop_client/airdrop_client_test.cpp +++ b/tests/unit/airdrop_client/airdrop_client_test.cpp @@ -33,8 +33,8 @@ TEST(AirdropClientTest, HeartbeatConnectionNeverSend) { std::this_thread::sleep_for(std::chrono::milliseconds(500)); auto lost_connections = client.getLostConnections(std::chrono::milliseconds(500)); - EXPECT_EQ(lost_connections.size(), NUM_AIRDROP_BOTTLES); - for (const auto& [bottle_index, time_since_last_heartbeat] : lost_connections) { + EXPECT_EQ(lost_connections.size(), NUM_AIRDROPS); + for (const auto& [airdrop_index, time_since_last_heartbeat] : lost_connections) { EXPECT_GE(time_since_last_heartbeat, std::chrono::milliseconds(500)); } } @@ -58,9 +58,9 @@ TEST(AirdropClientTest, ObcSendToPayload) { recv_ad_packet(payload_socket, &p, sizeof(packet_t)); if (num_received > 1) { // ignore first two because will be ack_mode ASSERT_EQ(p.header, ARM); - uint8_t bottle, state; - parseID(p.id, &bottle, &state); - ASSERT_EQ(bottle, UDP2_A); + uint8_t airdrop, state; + parseID(p.id, &airdrop, &state); + ASSERT_EQ(airdrop, UDP2_A); ASSERT_EQ(state, OBC_NULL); } num_received++; diff --git a/tests/unit/airdrop_client/mock_airdrop_sockets_test.cpp b/tests/unit/airdrop_client/mock_airdrop_sockets_test.cpp index 26a9b022..06096b38 100644 --- a/tests/unit/airdrop_client/mock_airdrop_sockets_test.cpp +++ b/tests/unit/airdrop_client/mock_airdrop_sockets_test.cpp @@ -21,9 +21,9 @@ TEST(MockAirdropSocketTest, TestSendMessageObcToPayload) { recv_ad_packet(payload_socket, &p, sizeof(packet_t)); mode_packet_t* mode_p = reinterpret_cast(&p); ASSERT_EQ(mode_p->header, SET_MODE); - uint8_t bottle, state; - parseID(mode_p->id, &bottle, &state); - ASSERT_EQ(bottle, UDP2_A); + uint8_t airdrop, state; + parseID(mode_p->id, &airdrop, &state); + ASSERT_EQ(airdrop, UDP2_A); ASSERT_EQ(state, OBC_NULL); ASSERT_EQ(mode_p->mode, GUIDED); ASSERT_EQ(mode_p->version, UDP2_VERSION); @@ -34,8 +34,8 @@ TEST(MockAirdropSocketTest, TestSendMessageObcToPayload) { recv_ad_packet(payload_socket, &p, sizeof(packet_t)); heartbeat_packet_t* hb_p = reinterpret_cast(&p); ASSERT_EQ(hb_p->header, HEARTBEAT); - parseID(hb_p->id, &bottle, &state); - ASSERT_EQ(bottle, UDP2_A); + parseID(hb_p->id, &airdrop, &state); + ASSERT_EQ(airdrop, UDP2_A); ASSERT_EQ(state, OBC_NULL); ASSERT_FLOAT_EQ(hb_p->payload_lat, 32.123); ASSERT_FLOAT_EQ(hb_p->payload_lng, 76.321); @@ -46,13 +46,12 @@ TEST(MockAirdropSocketTest, TestSendMessageObcToPayload) { send_ad_packet(obc_socket, makeHeartbeatPacket(UDP2_B, OBC_NULL, 0.2, 7.2, 6, 92)); send_ad_packet(obc_socket, makeHeartbeatPacket(UDP2_C, OBC_NULL, 0.3, 7.3, 7, 93)); send_ad_packet(obc_socket, makeHeartbeatPacket(UDP2_D, OBC_NULL, 0.4, 7.4, 8, 94)); - send_ad_packet(obc_socket, makeHeartbeatPacket(UDP2_E, OBC_NULL, 0.5, 7.5, 9, 95)); recv_ad_packet(payload_socket, &p, sizeof(packet_t)); hb_p = reinterpret_cast(&p); ASSERT_EQ(hb_p->header, HEARTBEAT); - parseID(hb_p->id, &bottle, &state); - ASSERT_EQ(bottle, UDP2_A); + parseID(hb_p->id, &airdrop, &state); + ASSERT_EQ(airdrop, UDP2_A); ASSERT_EQ(state, OBC_NULL); ASSERT_FLOAT_EQ(hb_p->payload_lat, 0.1); ASSERT_FLOAT_EQ(hb_p->payload_lng, 7.1); @@ -62,8 +61,8 @@ TEST(MockAirdropSocketTest, TestSendMessageObcToPayload) { recv_ad_packet(payload_socket, &p, sizeof(packet_t)); hb_p = reinterpret_cast(&p); ASSERT_EQ(hb_p->header, HEARTBEAT); - parseID(hb_p->id, &bottle, &state); - ASSERT_EQ(bottle, UDP2_B); + parseID(hb_p->id, &airdrop, &state); + ASSERT_EQ(airdrop, UDP2_B); ASSERT_EQ(state, OBC_NULL); ASSERT_FLOAT_EQ(hb_p->payload_lat, 0.2); ASSERT_FLOAT_EQ(hb_p->payload_lng, 7.2); @@ -73,8 +72,8 @@ TEST(MockAirdropSocketTest, TestSendMessageObcToPayload) { recv_ad_packet(payload_socket, &p, sizeof(packet_t)); hb_p = reinterpret_cast(&p); ASSERT_EQ(hb_p->header, HEARTBEAT); - parseID(hb_p->id, &bottle, &state); - ASSERT_EQ(bottle, UDP2_C); + parseID(hb_p->id, &airdrop, &state); + ASSERT_EQ(airdrop, UDP2_C); ASSERT_EQ(state, OBC_NULL); ASSERT_FLOAT_EQ(hb_p->payload_lat, 0.3); ASSERT_FLOAT_EQ(hb_p->payload_lng, 7.3); @@ -84,8 +83,8 @@ TEST(MockAirdropSocketTest, TestSendMessageObcToPayload) { recv_ad_packet(payload_socket, &p, sizeof(packet_t)); hb_p = reinterpret_cast(&p); ASSERT_EQ(hb_p->header, HEARTBEAT); - parseID(hb_p->id, &bottle, &state); - ASSERT_EQ(bottle, UDP2_D); + parseID(hb_p->id, &airdrop, &state); + ASSERT_EQ(airdrop, UDP2_D); ASSERT_EQ(state, OBC_NULL); ASSERT_FLOAT_EQ(hb_p->payload_lat, 0.4); ASSERT_FLOAT_EQ(hb_p->payload_lng, 7.4); @@ -95,8 +94,8 @@ TEST(MockAirdropSocketTest, TestSendMessageObcToPayload) { recv_ad_packet(payload_socket, &p, sizeof(packet_t)); hb_p = reinterpret_cast(&p); ASSERT_EQ(hb_p->header, HEARTBEAT); - parseID(hb_p->id, &bottle, &state); - ASSERT_EQ(bottle, UDP2_E); + parseID(hb_p->id, &airdrop, &state); + ASSERT_EQ(airdrop, UDP2_D); ASSERT_EQ(state, OBC_NULL); ASSERT_FLOAT_EQ(hb_p->payload_lat, 0.5); ASSERT_FLOAT_EQ(hb_p->payload_lng, 7.5); diff --git a/tests/unit/airdrop_client/packet_queue_test.cpp b/tests/unit/airdrop_client/packet_queue_test.cpp index 53ef0e18..5693c9ad 100644 --- a/tests/unit/airdrop_client/packet_queue_test.cpp +++ b/tests/unit/airdrop_client/packet_queue_test.cpp @@ -65,7 +65,7 @@ TEST(PacketQueueTest, PushUntilFullThenPopUntilEmpty) { pqueue_init(&q); for (int i = 0; i < MAX_PACKETS; i++) { - packet_t p = makeResetPacket(static_cast(i)); + packet_t p = makeResetPacket(static_cast(i)); pqueue_push(&q, p); ASSERT_FALSE(pqueue_empty(&q)); } @@ -95,13 +95,13 @@ TEST(PacketQueueTest, SimulateNormalUse) { temp = pqueue_pop(&q); ASSERT_EQ(temp.header, RESET); - uint8_t bottle, state; - parseID(temp.id, &bottle, &state); - ASSERT_EQ(bottle, UDP2_D); + uint8_t airdrop, state; + parseID(temp.id, &airdrop, &state); + ASSERT_EQ(airdrop, UDP2_D); temp = pqueue_pop(&q); ASSERT_EQ(temp.header, RESET); - parseID(temp.id, &bottle, &state); - ASSERT_EQ(bottle, UDP2_D); + parseID(temp.id, &airdrop, &state); + ASSERT_EQ(airdrop, UDP2_D); for (int i = BURST; i < BURST * 2; i++) { pqueue_push(&q, makeResetPacket(UDP2_D)); @@ -109,12 +109,12 @@ TEST(PacketQueueTest, SimulateNormalUse) { temp = pqueue_pop(&q); ASSERT_EQ(temp.header, RESET); - parseID(temp.id, &bottle, &state); - ASSERT_EQ(bottle, UDP2_D); + parseID(temp.id, &airdrop, &state); + ASSERT_EQ(airdrop, UDP2_D); temp = pqueue_pop(&q); ASSERT_EQ(temp.header, RESET); - parseID(temp.id, &bottle, &state); - ASSERT_EQ(bottle, UDP2_D); + parseID(temp.id, &airdrop, &state); + ASSERT_EQ(airdrop, UDP2_D); for (int i = BURST * 2; i < BURST * 3; i++) { pqueue_push(&q, makeResetPacket(UDP2_D)); @@ -123,8 +123,8 @@ TEST(PacketQueueTest, SimulateNormalUse) { for (int i = 4; i < BURST * 3; i++) { temp = pqueue_pop(&q); ASSERT_EQ(temp.header, RESET); - parseID(temp.id, &bottle, &state); - ASSERT_EQ(bottle, UDP2_D); + parseID(temp.id, &airdrop, &state); + ASSERT_EQ(airdrop, UDP2_D); } ASSERT_TRUE(pqueue_empty(&q)); @@ -157,7 +157,7 @@ TEST(PacketQueueTest, PopWaitMultiThreaded) { packet_t p = pqueue_wait_pop(&q); ASSERT_EQ(p.header, RESET); - uint8_t bottle, state; - parseID(p.id, &bottle, &state); - ASSERT_EQ(bottle, UDP2_C); + uint8_t airdrop, state; + parseID(p.id, &airdrop, &state); + ASSERT_EQ(airdrop, UDP2_C); } \ No newline at end of file diff --git a/tests/unit/cv/utilities_test.cpp b/tests/unit/cv/utilities_test.cpp index 1be7f837..873c7712 100644 --- a/tests/unit/cv/utilities_test.cpp +++ b/tests/unit/cv/utilities_test.cpp @@ -60,5 +60,5 @@ TEST(CVUtilities, CropValidAndInvalidSizes) { EXPECT_EQ(cropped.rows, testCase.expectedCroppedImg.rows); EXPECT_EQ(cropped.cols, testCase.expectedCroppedImg.cols); - }; + } } diff --git a/tests/unit/gcs_server_test.cpp b/tests/unit/gcs_server_test.cpp index 13d17b13..7635187f 100644 --- a/tests/unit/gcs_server_test.cpp +++ b/tests/unit/gcs_server_test.cpp @@ -1,206 +1,206 @@ -#include -#include - -#include -#include -#include -#include - -#include - -#include "network/gcs.hpp" -#include "network/gcs_routes.hpp" -#include "network/gcs_macros.hpp" -#include "core/mission_state.hpp" -#include "resources/json_snippets.hpp" -#include "utilities/http.hpp" -#include "utilities/obc_config.hpp" -#include "ticks/mission_prep.hpp" -#include "ticks/path_gen.hpp" -#include "ticks/mav_upload.hpp" -#include "ticks/tick.hpp" - -#include "handler_params.hpp" - -// Might have to change this later on if we preload -// mission from file -TEST(GCSServerTest, GetMissionNoMission) { - DECLARE_HANDLER_PARAMS(state, req, resp); - - GCS_HANDLE(Get, mission)(state, req, resp); - - EXPECT_EQ(BAD_REQUEST, resp.status); - EXPECT_EQ(state->mission_params.getCachedMission(), std::nullopt); -} - -TEST(GCSServerTest, PostBadMission) { - DECLARE_HANDLER_PARAMS(state, req, resp); - req.body = resources::mission_json_bad_1; - - GCS_HANDLE(Post, mission)(state, req, resp); - EXPECT_EQ(resp.status, BAD_REQUEST); - GCS_HANDLE(Get, mission)(state, req, resp); - EXPECT_EQ(resp.status, BAD_REQUEST); -} - -// TODO: can probably cut down on the amount of code copy/paste that is going on here -TEST(GCSServerTest, PostMissionThenGetMission) { - DECLARE_HANDLER_PARAMS(state, req, resp); - req.body = resources::mission_json_good_1; - nlohmann::json request_mission = nlohmann::json::parse(req.body); - - GCS_HANDLE(Post, mission)(state, req, resp); - - // ======================================================================== - // Verify all of the internal state is correct - - EXPECT_EQ(OK, resp.status); - auto cartesian = state->getCartesianConverter().value(); - - auto cached_airdrop_bounds = state->mission_params.getAirdropBoundary(); - auto request_airdrop_bounds = request_mission.at("AirdropBoundary"); - EXPECT_EQ(cached_airdrop_bounds.size(), request_airdrop_bounds.size()); - for (int i = 0; i < cached_airdrop_bounds.size(); i++) { - auto cached_coord = cartesian.toLatLng(cached_airdrop_bounds[i]); - auto request_coord = request_airdrop_bounds[i]; - auto request_lat = request_coord.at("Latitude").get(); - auto request_lng = request_coord.at("Longitude").get(); - EXPECT_FLOAT_EQ(request_lat, cached_coord.latitude()); - EXPECT_FLOAT_EQ(request_lng, cached_coord.longitude()); - } - auto cached_flight_bounds = state->mission_params.getFlightBoundary(); - auto request_flight_bounds = request_mission.at("FlightBoundary"); - EXPECT_EQ(cached_flight_bounds.size(), request_flight_bounds.size()); - for (int i = 0; i < cached_flight_bounds.size(); i++) { - auto cached_coord = cartesian.toLatLng(cached_flight_bounds[i]); - auto request_coord = request_flight_bounds[i]; - auto request_lat = request_coord.at("Latitude").get(); - auto request_lng = request_coord.at("Longitude").get(); - EXPECT_FLOAT_EQ(request_lat, cached_coord.latitude()); - EXPECT_FLOAT_EQ(request_lng, cached_coord.longitude()); - } - auto cached_waypoints = state->mission_params.getWaypoints(); - auto request_waypoints = request_mission.at("Waypoints"); - EXPECT_EQ(cached_waypoints.size(), request_waypoints.size()); - for (int i = 0; i < cached_waypoints.size(); i++) { - auto cached_coord = cartesian.toLatLng(cached_waypoints[i]); - auto request_coord = request_waypoints[i]; - auto request_lat = request_coord.at("Latitude").get(); - auto request_lng = request_coord.at("Longitude").get(); - auto request_alt = request_coord.at("Altitude").get(); - EXPECT_FLOAT_EQ(request_lat, cached_coord.latitude()); - EXPECT_FLOAT_EQ(request_lng, cached_coord.longitude()); - EXPECT_FLOAT_EQ(request_alt, cached_coord.altitude()); - } - auto cached_bottles = state->mission_params.getAirdropBottles(); - auto request_bottles = request_mission.at("BottleAssignments"); - EXPECT_EQ(cached_bottles.size(), NUM_AIRDROP_BOTTLES); - EXPECT_EQ(request_bottles.size(), NUM_AIRDROP_BOTTLES); - // Create a map from the index to the cached version of the bottle, - // and the version of the bottle that was sent in the request - std::unordered_map indexToCached; - std::unordered_map indexToRequest; - for (int i = 0; i < NUM_AIRDROP_BOTTLES; i++) { - Bottle cached = cached_bottles[i]; - Bottle request; - google::protobuf::util::JsonStringToMessage(request_bottles.at(i).dump(), &request); - indexToCached.insert({cached.index(), cached}); - indexToRequest.insert({request.index(), request}); - } - // Now it is easy to compare bottles of the same drop index - for (int i = BottleDropIndex::A; i <= BottleDropIndex::E; i++) { - auto index = static_cast(i); - Bottle cached = indexToCached.at(index); - Bottle request = indexToRequest.at(index); - EXPECT_EQ(cached.ismannikin(), request.ismannikin()); - if (cached.ismannikin()) { - // only check the other parameters if they are relevant, - // i.e. this is not the emergent (mannequin) target - EXPECT_EQ(cached.alphanumeric(), request.alphanumeric()); - EXPECT_EQ(cached.alphanumericcolor(), request.alphanumericcolor()); - EXPECT_EQ(cached.shape(), request.shape()); - EXPECT_EQ(cached.shapecolor(), request.shapecolor()); - } - } - - // ======================================================================== - // Verify we can get out the same mission json again - - req = httplib::Request(); - resp = httplib::Response(); - - GCS_HANDLE(Get, mission)(state, req, resp); - - auto response_mission = nlohmann::json::parse(resp.body); - - EXPECT_EQ(response_mission.at("Waypoints"), request_mission.at("Waypoints")); - EXPECT_EQ(response_mission.at("AirdropBoundary"), request_mission.at("AirdropBoundary")); - EXPECT_EQ(response_mission.at("FlightBoundary"), request_mission.at("FlightBoundary")); - - // Have to compare these specifically because the translation alters how some of the fields - // look. E.g. on the request we specify indices with letters, but after protobuf parses - // it, it then stores the index as an integer. - auto response_bottles = response_mission.at("BottleAssignments"); - // already made request_bottles above, so can reuse it here - EXPECT_EQ(request_bottles.size(), response_bottles.size()); - // already made indexToRequest above, so can reuse it here - std::unordered_map indexToResponse; - for (int i = 0; i < response_bottles.size(); i++) { - Bottle response; - google::protobuf::util::JsonStringToMessage(response_bottles[i].dump(), &response); - indexToResponse.insert({response.index(), response}); - } - for (int i = BottleDropIndex::A; i <= BottleDropIndex::E; i++) { - auto index = static_cast(i); - Bottle response = indexToResponse.at(index); - Bottle request = indexToRequest.at(index); - EXPECT_EQ(response.ismannikin(), request.ismannikin()); - if (response.ismannikin()) { - // only check the other parameters if they are relevant, - // i.e. this is not the emergent (mannequin) target - EXPECT_EQ(response.alphanumeric(), request.alphanumeric()); - EXPECT_EQ(response.alphanumericcolor(), request.alphanumericcolor()); - EXPECT_EQ(response.shape(), request.shape()); - EXPECT_EQ(response.shapecolor(), request.shapecolor()); - } - } -} - -TEST(GCSServerTest, GetInitialPathNoPath) { - DECLARE_HANDLER_PARAMS(state, req, resp); - - EXPECT_EQ(state->getInitPath().get().size(), 0); - - GCS_HANDLE(Get, path, initial)(state, req, resp); - - EXPECT_EQ(resp.status, BAD_REQUEST); - EXPECT_EQ(state->getInitPath().get().size(), 0); -} - -/* - * Test fails to get the models (e.g. siamese.pt), but we are no longer using - * them anyways. - * - * However, we still should test state transitions after we remove old CV code -TEST(GCSServerTest, SetupStateTransitions) { - // First upload a mission so that we generate a path - DECLARE_HANDLER_PARAMS(state, req, resp); - req.body = resources::mission_json_good_1; - state->setTick(new MissionPrepTick(state)); - - GCS_HANDLE(Post, mission)(state, req, resp); - - state->doTick(); - EXPECT_EQ(state->getTickID(), TickID::PathGen); - do { // wait for path to generate - auto wait = state->doTick(); - std::this_thread::sleep_for(wait); - } while (state->getInitPath().get().empty()); - // have an initial path, but waiting for validation - EXPECT_FALSE(state->getInitPath().get().empty()); - EXPECT_EQ(state->getTickID(), TickID::PathValidate); - - // todo: figure out way to mock the mav connection - // so we can validate the path and mock mission upload -} -*/ \ No newline at end of file +// #include +// #include + +// #include +// #include +// #include +// #include + +// #include + +// #include "network/gcs.hpp" +// #include "network/gcs_routes.hpp" +// #include "network/gcs_macros.hpp" +// #include "core/mission_state.hpp" +// #include "resources/json_snippets.hpp" +// #include "utilities/http.hpp" +// #include "utilities/obc_config.hpp" +// #include "ticks/mission_prep.hpp" +// #include "ticks/path_gen.hpp" +// #include "ticks/mav_upload.hpp" +// #include "ticks/tick.hpp" + +// #include "handler_params.hpp" + +// // Might have to change this later on if we preload +// // mission from file +// TEST(GCSServerTest, GetMissionNoMission) { +// DECLARE_HANDLER_PARAMS(state, req, resp); + +// GCS_HANDLE(Get, mission)(state, req, resp); + +// EXPECT_EQ(BAD_REQUEST, resp.status); +// EXPECT_EQ(state->mission_params.getCachedMission(), std::nullopt); +// } + +// TEST(GCSServerTest, PostBadMission) { +// DECLARE_HANDLER_PARAMS(state, req, resp); +// req.body = resources::mission_json_bad_1; + +// GCS_HANDLE(Post, mission)(state, req, resp); +// EXPECT_EQ(resp.status, BAD_REQUEST); +// GCS_HANDLE(Get, mission)(state, req, resp); +// EXPECT_EQ(resp.status, BAD_REQUEST); +// } + +// // TODO: can probably cut down on the amount of code copy/paste that is going on here +// TEST(GCSServerTest, PostMissionThenGetMission) { +// DECLARE_HANDLER_PARAMS(state, req, resp); +// req.body = resources::mission_json_good_1; +// nlohmann::json request_mission = nlohmann::json::parse(req.body); + +// GCS_HANDLE(Post, mission)(state, req, resp); + +// // ======================================================================== +// // Verify all of the internal state is correct + +// EXPECT_EQ(OK, resp.status); +// auto cartesian = state->getCartesianConverter().value(); + +// auto cached_airdrop_bounds = state->mission_params.getAirdropBoundary(); +// auto request_airdrop_bounds = request_mission.at("AirdropBoundary"); +// EXPECT_EQ(cached_airdrop_bounds.size(), request_airdrop_bounds.size()); +// for (int i = 0; i < cached_airdrop_bounds.size(); i++) { +// auto cached_coord = cartesian.toLatLng(cached_airdrop_bounds[i]); +// auto request_coord = request_airdrop_bounds[i]; +// auto request_lat = request_coord.at("Latitude").get(); +// auto request_lng = request_coord.at("Longitude").get(); +// EXPECT_FLOAT_EQ(request_lat, cached_coord.latitude()); +// EXPECT_FLOAT_EQ(request_lng, cached_coord.longitude()); +// } +// auto cached_flight_bounds = state->mission_params.getFlightBoundary(); +// auto request_flight_bounds = request_mission.at("FlightBoundary"); +// EXPECT_EQ(cached_flight_bounds.size(), request_flight_bounds.size()); +// for (int i = 0; i < cached_flight_bounds.size(); i++) { +// auto cached_coord = cartesian.toLatLng(cached_flight_bounds[i]); +// auto request_coord = request_flight_bounds[i]; +// auto request_lat = request_coord.at("Latitude").get(); +// auto request_lng = request_coord.at("Longitude").get(); +// EXPECT_FLOAT_EQ(request_lat, cached_coord.latitude()); +// EXPECT_FLOAT_EQ(request_lng, cached_coord.longitude()); +// } +// auto cached_waypoints = state->mission_params.getWaypoints(); +// auto request_waypoints = request_mission.at("Waypoints"); +// EXPECT_EQ(cached_waypoints.size(), request_waypoints.size()); +// for (int i = 0; i < cached_waypoints.size(); i++) { +// auto cached_coord = cartesian.toLatLng(cached_waypoints[i]); +// auto request_coord = request_waypoints[i]; +// auto request_lat = request_coord.at("Latitude").get(); +// auto request_lng = request_coord.at("Longitude").get(); +// auto request_alt = request_coord.at("Altitude").get(); +// EXPECT_FLOAT_EQ(request_lat, cached_coord.latitude()); +// EXPECT_FLOAT_EQ(request_lng, cached_coord.longitude()); +// EXPECT_FLOAT_EQ(request_alt, cached_coord.altitude()); +// } +// auto cached_airdrops = state->mission_params.getAirdrops(); +// auto request_airdrops = request_mission.at("AirdropAssignments"); +// EXPECT_EQ(cached_airdrops.size(), NUM_AIRDROPS); +// EXPECT_EQ(reuqest_airdrops.size(), NUM_AIRDROPS); +// // Create a map from the index to the cached version of the bottle, +// // and the version of the bottle that was sent in the request +// std::unordered_map indexToCached; +// std::unordered_map indexToRequest; +// for (int i = 0; i < NUM_AIRDROPS; i++) { +// Airdrop cached = cached_airdrops[i]; +// Airdrop request; +// google::protobuf::util::JsonStringToMessage(request_airdrops.at(i).dump(), &request); +// indexToCached.insert({cached.index(), cached}); +// indexToRequest.insert({request.index(), request}); +// } +// // Now it is easy to compare bottles of the same drop index +// for (int i = AirdropIndex::Kaz; i <= AirdropIndex::Daniel; i++) { +// auto index = static_cast(i); +// Airdrop cached = indexToCached.at(index); +// Airdrop request = indexToRequest.at(index); +// EXPECT_EQ(cached.ismannikin(), request.ismannikin()); +// if (cached.ismannikin()) { +// // only check the other parameters if they are relevant, +// // i.e. this is not the emergent (mannequin) target +// EXPECT_EQ(cached.alphanumeric(), request.alphanumeric()); +// EXPECT_EQ(cached.alphanumericcolor(), request.alphanumericcolor()); +// EXPECT_EQ(cached.shape(), request.shape()); +// EXPECT_EQ(cached.shapecolor(), request.shapecolor()); +// } +// } + +// // ======================================================================== +// // Verify we can get out the same mission json again + +// req = httplib::Request(); +// resp = httplib::Response(); + +// GCS_HANDLE(Get, mission)(state, req, resp); + +// auto response_mission = nlohmann::json::parse(resp.body); + +// EXPECT_EQ(response_mission.at("Waypoints"), request_mission.at("Waypoints")); +// EXPECT_EQ(response_mission.at("AirdropBoundary"), request_mission.at("AirdropBoundary")); +// EXPECT_EQ(response_mission.at("FlightBoundary"), request_mission.at("FlightBoundary")); + +// // Have to compare these specifically because the translation alters how some of the fields +// // look. E.g. on the request we specify indices with letters, but after protobuf parses +// // it, it then stores the index as an integer. +// auto response_airdrops = response_mission.at("AirdropAssignments"); +// // already made request_bottles above, so can reuse it here +// EXPECT_EQ(request_airdrops.size(), response_airdrops.size()); +// // already made indexToRequest above, so can reuse it here +// std::unordered_map indexToResponse; +// for (int i = 0; i < response_airdrops.size(); i++) { +// Airdrop response; +// google::protobuf::util::JsonStringToMessage(response_airdrops[i].dump(), &response); +// indexToResponse.insert({response.index(), response}); +// } +// for (int i = AirdropIndex::Kaz; i <= AirdropIndex::Daniel; i++) { +// auto index = static_cast(i); +// Airdrop response = indexToResponse.at(index); +// Airdrop request = indexToRequest.at(index); +// EXPECT_EQ(response.ismannikin(), request.ismannikin()); +// if (response.ismannikin()) { +// // only check the other parameters if they are relevant, +// // i.e. this is not the emergent (mannequin) target +// EXPECT_EQ(response.alphanumeric(), request.alphanumeric()); +// EXPECT_EQ(response.alphanumericcolor(), request.alphanumericcolor()); +// EXPECT_EQ(response.shape(), request.shape()); +// EXPECT_EQ(response.shapecolor(), request.shapecolor()); +// } +// } +// } + +// TEST(GCSServerTest, GetInitialPathNoPath) { +// DECLARE_HANDLER_PARAMS(state, req, resp); + +// EXPECT_EQ(state->getInitPath().get().size(), 0); + +// GCS_HANDLE(Get, path, initial)(state, req, resp); + +// EXPECT_EQ(resp.status, BAD_REQUEST); +// EXPECT_EQ(state->getInitPath().get().size(), 0); +// } + +// /* +// * Test fails to get the models (e.g. siamese.pt), but we are no longer using +// * them anyways. +// * +// * However, we still should test state transitions after we remove old CV code +// TEST(GCSServerTest, SetupStateTransitions) { +// // First upload a mission so that we generate a path +// DECLARE_HANDLER_PARAMS(state, req, resp); +// req.body = resources::mission_json_good_1; +// state->setTick(new MissionPrepTick(state)); + +// GCS_HANDLE(Post, mission)(state, req, resp); + +// state->doTick(); +// EXPECT_EQ(state->getTickID(), TickID::PathGen); +// do { // wait for path to generate +// auto wait = state->doTick(); +// std::this_thread::sleep_for(wait); +// } while (state->getInitPath().get().empty()); +// // have an initial path, but waiting for validation +// EXPECT_FALSE(state->getInitPath().get().empty()); +// EXPECT_EQ(state->getTickID(), TickID::PathValidate); + +// // todo: figure out way to mock the mav connection +// // so we can validate the path and mock mission upload +// } +// */ \ No newline at end of file diff --git a/tests/unit/udp2_squared/parse_id_test.cpp b/tests/unit/udp2_squared/parse_id_test.cpp index 49ecfd65..c2147965 100644 --- a/tests/unit/udp2_squared/parse_id_test.cpp +++ b/tests/unit/udp2_squared/parse_id_test.cpp @@ -3,16 +3,15 @@ extern "C" { #include "udp_squared/protocol.h" } - TEST(UDP2Test, TestIDParse) { uint8_t id = makeID(UDP2_C, OBC_NULL); - uint8_t bottle, state; - parseID(id, &bottle, &state); - ASSERT_EQ(bottle, UDP2_C); + uint8_t airdrop, state; + parseID(id, &airdrop, &state); + ASSERT_EQ(airdrop, UDP2_C); ASSERT_EQ(state, OBC_NULL); id = makeID(UDP2_B, static_cast(1)); - parseID(id, &bottle, &state); - ASSERT_EQ(bottle, UDP2_B); + parseID(id, &airdrop, &state); + ASSERT_EQ(airdrop, UDP2_B); ASSERT_EQ(state, 1); } \ No newline at end of file