Implement YOLO, Preprocess, Pipeline, Aggregator, update Mapping

.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
 

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
+)

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
     },

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
     },

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()

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
 

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

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:

include/core/mission_parameters.hpp

@@ -9,8 +9,9 @@
 #include <unordered_map>
 #include <vector>
 
+// 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<Bottle> getAirdropBottles();
+    // CHANGE: Return type is now std::vector<Airdrop>
+    std::vector<Airdrop> 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<Polygon, Polygon, Polyline, std::vector<Bottle>> getConfig();
+    // CHANGE: Tuple element type is now std::vector<Airdrop>
+    std::tuple<Polygon, Polygon, Polygon, Polyline, std::vector<Airdrop>> getConfig();
 
     // returns error string to be displayed back to the user
+    // Ensure CartesianConverter<GPSProtoVec> template parameter matches usage
     std::optional<std::string> setMission(Mission, CartesianConverter<GPSProtoVec>);
 
     void saveToFile(std::string filename);
@@ -50,13 +55,16 @@ class MissionParameters {
 
     Polygon flightBoundary;
     Polygon airdropBoundary;
+    Polygon mappingBoundary;
     Polyline waypoints;
-    std::vector<Bottle> bottles;
+    // CHANGE: Member variable type is now std::vector<Airdrop>
+    std::vector<Airdrop> airdrops;
 
     std::optional<Mission> 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_

include/core/mission_state.hpp

@@ -3,6 +3,7 @@
 
 #include <array>
 #include <chrono>
+#include <cmath>
 #include <functional>
 #include <memory>
 #include <mutex>
@@ -52,8 +53,8 @@ class MissionState {
     void setAirdropPath(const MissionPath& airdrop_path);
     MissionPath getAirdropPath();
 
-    void markBottleAsDropped(BottleDropIndex bottle);
-    std::unordered_set<BottleDropIndex> getDroppedBottles();
+    void markAirdropAsDropped(AirdropIndex airdrop);
+    std::unordered_set<AirdropIndex> getDroppedAirdrops();
 
     /*
      * Gets a locking reference to the underlying tick for the given tick subclass T.
@@ -103,12 +104,15 @@ class MissionState {
     std::shared_ptr<CameraInterface> getCamera();
     void setCamera(std::shared_ptr<CameraInterface> 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<bottle_t> next_bottle_to_drop;
+    std::optional<airdrop_t> next_airdrop_to_drop;
 
  private:
     std::mutex converter_mut;
@@ -124,20 +128,22 @@ class MissionState {
     std::mutex airdrop_path_mut;
     MissionPath airdrop_path;
 
-    std::mutex dropped_bottles_mut;
-    std::unordered_set<BottleDropIndex> dropped_bottles;
+    std::mutex dropped_airdrops_mut;
+    std::unordered_set<AirdropIndex> dropped_airdrops;
 
     std::shared_ptr<MavlinkClient> mav;
     std::shared_ptr<AirdropClient> airdrop;
     std::shared_ptr<CVAggregator> cv;
     std::shared_ptr<CameraInterface> camera;
 
     std::mutex cv_mut;
+    // Represents a single detected target used in pipeline
     std::vector<DetectedTarget> 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<size_t, NUM_AIRDROP_BOTTLES> cv_matches;
+    std::array<size_t, NUM_AIRDROPS> cv_matches;
 
+    bool mappingIsDone;
 
     void _setTick(Tick* newTick);  // does not acquire the tick_mut
 };

include/cv/aggregator.hpp

@@ -1,38 +1,55 @@
 #ifndef INCLUDE_CV_AGGREGATOR_HPP_
 #define INCLUDE_CV_AGGREGATOR_HPP_
 
-#include <mutex>
-#include <memory>
-#include <vector>
+#include <atomic>
+#include <cmath>
+#include <functional>
 #include <future>
+#include <memory>
+#include <mutex>
 #include <queue>
-#include <functional>
+#include <thread>
 #include <unordered_map>
+#include <vector>
 
-#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<DetectedTarget> detected_targets;
-    // mapping from bottle -> index into detected_targets
-    // (optional is none if we don't have a match yet)
-    std::unordered_map<BottleDropIndex, std::optional<size_t>> 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<Bbox> bboxes;      // All bounding boxes in that image
+    std::vector<GPSCoord> coords;  // Matching lat-longs for each bounding box
 };
 
+struct CVResults {
+    std::vector<AggregatedRun> runs;  // Each pipeline invocation => 1 run
+};
 
+struct MatchedResults {
+    std::unordered_map<AirdropIndex, AirdropTarget> 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<CVResults> getResults();
 
+    // Lockable pointer to retrieve matched results (after manual match)
+    LockPtr<MatchedResults> getMatchedResults();
+
+    // For the endpoint to reset the current list of structs
+    std::vector<AggregatedRun> 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<ImageData> overflow_queue;
 
+    // Shared aggregator results
     std::shared_ptr<CVResults> results;
+
+    // Shared matched results
+    std::shared_ptr<MatchedResults> matched_results;
 };
 
 #endif  // INCLUDE_CV_AGGREGATOR_HPP_