Implement Flysearch + CVLoiter Tick (#191)

* Added function prototype

* Modified TODOs

* added prototype annotations for CVLoiter

* Added CVLoiter constructor

* Added CV Aggregator

* Moved inits to header file

* fixed header, added comments

* stashed changes before merge main

* Fixed some constructor references

* Added Saliency model path

* Made hierachy changes (Moved CV setup to mission_prep)

* added flysearch fixes

* Added Bottle comparison prototype

* Finalized Both ticks

* Fixed some compiling errors

* added more comments, removed redundent inits

* code review & misc fixes

* fixes for lint

* fix test and lint

---------

Co-authored-by: Tyler Lentz <[email protected]>

.devcontainer/devcontainer.json

@@ -9,9 +9,9 @@
 
 	// Enable network mode host if on Linux and testing airdrop connectivity
     // "appPort": [ "45906:45906/udp", "45907:45907/udp" ], // port forward airdrop ports for local testing
-	// "runArgs": [
-	// 	"--network=host"
-	// ],
+	"runArgs": [
+		"--network=host"
+	],
 
 	"extensions.verifySignature": false,
 

CMakeLists.txt

@@ -198,6 +198,7 @@ if(CPPLINT)
     # Do not require licenses, TODO assignment, Google versions of C++ libs
     # also don't check for runtime/references since Google's public style guidelines are behind https://github.com/cpplint/cpplint/issues/148
     --exclude=../include/udp_squared
+    --exclude=../src/utilities/base64.cpp
     --filter=-legal,-readability/todo,-build/c++11,-runtime/references
     --linelength=100
     --recursive

configs/config.json

@@ -1,6 +1,6 @@
 {
     "logging": {
-        "dir": "/workspaces/obcpp/logs"
+        "dir": "/obcpp/logs"
     },
     "network": {
         "mavlink": {
@@ -37,6 +37,11 @@
             "unguided_drop_distance_m": 50.0
         }
     },
+    "cv": {
+        "matching_model_dir": "/obcpp/models/target_siamese_1.pt",
+        "segmentation_model_dir": "/obcpp/models/fcn.pth",
+        "saliency_model_dir": "/obcpp/models/torchscript_19.pth"
+    },
     "camera": {
         "_comment": "See CameraConfig struct in datatypes.hpp for detailed explanations",
         "type": "lucid",

configs/dev-config.json

@@ -37,6 +37,11 @@
             "unguided_drop_distance_m": 50.0
         }
     },
+    "cv": {
+        "matching_model_dir": "/workspaces/obcpp/models/target_siamese_1.pt",
+        "segmentation_model_dir": "/workspaces/obcpp/models/fcn.pth",
+        "saliency_model_dir": "/workspaces/obcpp/models/torchscript_19.pth"
+    },
     "camera": {
         "_comment": "See CameraConfig struct in datatypes.hpp for detailed explanations",
         "type": "mock",

include/camera/lucid.hpp

@@ -74,6 +74,7 @@ class LucidCamera : public CameraInterface {
     */
     std::optional<ImageData> takePicture(const std::chrono::milliseconds& timeout,
         std::shared_ptr<MavlinkClient> mavlinkClient) override;
+
     void startStreaming() override;
 
  private:

include/core/mission_state.hpp

@@ -30,7 +30,7 @@ class Tick;
 
 class MissionState {
  public:
-    MissionState();
+    explicit MissionState(OBCConfig config);
     ~MissionState();
 
     const std::optional<CartesianConverter<GPSProtoVec>>& getCartesianConverter();
@@ -44,6 +44,9 @@ class MissionState {
     void setInitPath(std::vector<GPSCoord> init_path);
     const std::vector<GPSCoord>& getInitPath();
 
+    void setSearchPath(std::vector<GPSCoord> search_path);
+    const std::vector<GPSCoord>& getSearchPath();
+
     /*
      * Gets a locking reference to the underlying tick for the given tick subclass T.
      *
@@ -93,10 +96,8 @@ class MissionState {
     void setCamera(std::shared_ptr<CameraInterface> camera);
 
     MissionParameters mission_params;  // has its own mutex
-    RRTConfig rrt_config;
-    AirdropSearchConfig coverage_pathing_config;
-    CameraConfig camera_config;
-    float takeoff_alt_m;
+
+    OBCConfig config;
 
  private:
     std::mutex converter_mut;
@@ -106,7 +107,9 @@ class MissionState {
     std::shared_ptr<Tick> tick;
 
     std::mutex init_path_mut;  // for reading/writing the initial path
+    std::mutex search_path_mut;  // for reading/writing the search path
     std::vector<GPSCoord> init_path;
+    std::vector<GPSCoord> search_path;
 
     std::shared_ptr<MavlinkClient> mav;
     std::shared_ptr<AirdropClient> airdrop;

include/cv/aggregator.hpp

@@ -20,9 +20,12 @@ struct CVResults {
     std::unordered_map<BottleDropIndex, size_t> matches;
 };
 
+
+
 class CVAggregator {
  public:
-    explicit CVAggregator(Pipeline p);
+    explicit CVAggregator(Pipeline&& p);
+
     ~CVAggregator();
 
     void runPipeline(const ImageData& image);

include/network/gcs_routes.hpp

@@ -149,4 +149,16 @@ DEF_GCS_HANDLE(Post, takeoff, manual);
  */
 DEF_GCS_HANDLE(Post, takeoff, autonomous);
 
+/**
+ * POST /targets/validate
+ * ---
+ */
+DEF_GCS_HANDLE(Post, targets, validate);
+
+/**
+ * POST /targets/reject
+ * ---
+ */
+DEF_GCS_HANDLE(Post, targets, reject);
+
 #endif  // INCLUDE_NETWORK_GCS_ROUTES_HPP_

include/ticks/cv_loiter.hpp

@@ -5,20 +5,41 @@
 #include <chrono>
 
 #include "ticks/tick.hpp"
+#include <opencv2/opencv.hpp>
+#include <loguru.hpp>
+
+#include "cv/pipeline.hpp"
+#include "cv/aggregator.hpp"
+#include "protos/obc.pb.h"
+
 
 /*
- * Stop taking photos, loiter away from the search zone
- * and wait until CV processing is done. 
+ * loiter away from the search zone and wait until CV processing is done. 
+ * If CV Results are validated, proceed to Airdrop Prep.
  * 
  * See https://tritonuas.github.io/wiki/software/obc/tick_architecture/ticks/cvloiter/
  */
+
 class CVLoiterTick : public Tick {
  public:
-    explicit CVLoiterTick(std::shared_ptr<MissionState> state);
+    /* 
+     * Status of the CV processing
+     * (Will be) Modified through GCS Manual Control
+    */
+    enum class Status {
+        None = 0,
+        Validated = 1,
+        Rejected = 2,
+    };
 
+    explicit CVLoiterTick(std::shared_ptr<MissionState> state);
     std::chrono::milliseconds getWait() const override;
 
+    void setStatus(Status status);
     Tick* tick() override;
+
+ private:
+    Status status;
 };
 
 #endif  // INCLUDE_TICKS_CV_LOITER_HPP_

include/ticks/fly_search.hpp

@@ -6,19 +6,24 @@
 
 #include "ticks/tick.hpp"
 
+
 /*
  * Take photos and run CV pipeline while flying over
  * search region
  * 
  * See https://tritonuas.github.io/wiki/software/obc/tick_architecture/ticks/flysearch/
  */
+
 class FlySearchTick : public Tick {
  public:
     explicit FlySearchTick(std::shared_ptr<MissionState> state);
-
     std::chrono::milliseconds getWait() const override;
 
     Tick* tick() override;
+
+ private:
+    Polygon airdrop_boundary;
+    std::chrono::milliseconds last_photo_time;
 };
 
 #endif  // INCLUDE_TICKS_FLY_SEARCH_HPP_

include/ticks/mission_prep.hpp

@@ -6,6 +6,10 @@
 #include <string>
 
 #include "ticks/tick.hpp"
+#include "cv/pipeline.hpp"
+#include "cv/aggregator.hpp"
+
+#define NUMBOTTLES 5
 
 /*
  * Checks every second whether or not a valid mission has been uploaded.

include/utilities/obc_config.hpp

@@ -24,16 +24,19 @@ struct OBCConfig {
         float altitude_m;
     } takeoff;
 
+    struct {
+        std::string matching_model_dir;
+        std::string segmentation_model_dir;
+        std::string saliency_model_dir;
+    } cv;
+
     RRTConfig rrt_config;
     AirdropSearchConfig coverage_pathing_config;
     AirdropApproachConfig airdrop_pathing_config;
     CameraConfig camera_config;
 
     // Load user specified config json, or make a new one
     OBCConfig(int argc, char* argv[]);
-
- private:
-    void makeDefault();
 };
 
 #endif  // INCLUDE_UTILITIES_OBC_CONFIG_HPP_

src/camera/lucid.cpp

@@ -61,26 +61,15 @@ void LucidCamera::connect() {
 }
 
 LucidCamera::~LucidCamera() {
-  // aquire locks to Arena System and Device
-  WriteLock systemLock(this->arenaSystemLock);
-  WriteLock deviceLock(this->arenaDeviceLock);
-  CATCH_ARENA_EXCEPTION("closing Arena System",
-                        this->system->DestroyDevice(this->device);
-                        Arena::CloseSystem(this->system););
-}
-
-void LucidCamera::startStreaming() {
-    if (!this->isConnected()) {
-        LOG_F(ERROR, "LUCID Camera not connected. Cannot start streaming");
-        return;
-    }
+    // aquire locks to Arena System and Device
+    WriteLock systemLock(this->arenaSystemLock);
+    WriteLock deviceLock(this->arenaDeviceLock);
 
-    WriteLock lock(this->arenaDeviceLock);
-    CATCH_ARENA_EXCEPTION("starting stream",
-        this->device->StartStream(););
+    CATCH_ARENA_EXCEPTION("closing Arena System",
+        this->system->DestroyDevice(this->device);
+        Arena::CloseSystem(this->system););
 }
 
-
 void LucidCamera::startTakingPictures(const std::chrono::milliseconds& interval,
     std::shared_ptr<MavlinkClient> mavlinkClient) {
     if (this->isTakingPictures) {
@@ -328,6 +317,17 @@ std::optional<ImageData> LucidCamera::takePicture(const std::chrono::millisecond
     return {};
 }
 
+void LucidCamera::startStreaming() {
+    if (!this->isConnected()) {
+        LOG_F(ERROR, "LUCID Camera not connected. Cannot start streaming");
+        return;
+    }
+
+    WriteLock lock(this->arenaDeviceLock);
+    CATCH_ARENA_EXCEPTION("starting stream",
+        this->device->StartStream(););
+}
+
 std::optional<cv::Mat> LucidCamera::imgConvert(Arena::IImage* pImage) {
     CATCH_ARENA_EXCEPTION("converting Arena Image to OpenCV",
         // convert original image to BGR8 which is what opencv expects.

src/core/mission_state.cpp

@@ -16,9 +16,7 @@
 #include "utilities/logging.hpp"
 #include "utilities/obc_config.hpp"
 
-// in future might add to this
-MissionState::MissionState() {
-}
+MissionState::MissionState(OBCConfig config): config(config) {}
 
 // Need to explicitly define now that Tick is no longer an incomplete class
 // See:
@@ -78,6 +76,16 @@ const std::vector<GPSCoord>& MissionState::getInitPath() {
     return this->init_path;
 }
 
+void MissionState::setSearchPath(std::vector<GPSCoord> search_path) {
+    Lock lock(this->search_path_mut);
+    this->search_path = search_path;
+}
+
+const std::vector<GPSCoord>& MissionState::getSearchPath() {
+    Lock lock(this->search_path_mut);
+    return this->search_path;
+}
+
 std::shared_ptr<MavlinkClient> MissionState::getMav() { return this->mav; }
 
 void MissionState::setMav(std::shared_ptr<MavlinkClient> mav) { this->mav = mav; }

src/core/obc.cpp

@@ -4,6 +4,7 @@
 #include <cstdint>
 #include <future>
 
+#include "camera/mock.hpp"
 #include "camera/lucid.hpp"
 #include "core/obc.hpp"
 #include "core/mission_state.hpp"
@@ -22,12 +23,9 @@ extern "C" {
 OBC::OBC(OBCConfig config) {
     int gcs_port = config.network.gcs.port;
 
-    this->state = std::make_shared<MissionState>();
+    this->state = std::make_shared<MissionState>(config);
     this->state->setTick(new MissionPrepTick(this->state));
-    this->state->rrt_config = config.rrt_config;
-    this->state->camera_config = config.camera_config;
-    this->state->takeoff_alt_m = config.takeoff.altitude_m;
-    this->state->coverage_pathing_config = config.coverage_pathing_config;
+    this->state->config = config;
     this->gcs_server = std::make_unique<GCSServer>(gcs_port, this->state);
 
     // Don't need to look at these futures at all because the connect functions
@@ -37,13 +35,17 @@ OBC::OBC(OBCConfig config) {
         {this->connectMavlink(config.network.mavlink.connect);});
     this->connectAirdropThread = std::thread([this]{this->connectAirdrop();});
 
-    if (this->state->camera_config.type == "mock") {
-        this->state->setCamera(std::make_shared<MockCamera>(this->state->camera_config));
-    } else if (this->state->camera_config.type == "lucid") {
-        this->state->setCamera(std::make_shared<LucidCamera>(this->state->camera_config));
+    if (this->state->config.camera_config.type == "mock") {
+        this->state->setCamera(std::make_shared<MockCamera>(this->state->config.camera_config));
+    } else if (this->state->config.camera_config.type == "lucid") {
+        #ifdef ARENA_SDK_INSTALLED
+            this->state->setCamera(std::make_shared<LucidCamera>(this->state->config.camera_config));           //NOLINT
+        #else
+            LOG_F(FATAL, "Attempting to create Lucid Camera without having ArenaSDK installed.");
+        #endif
     } else {
         // default to mock if it's neither "mock" or "lucid"
-        this->state->setCamera(std::make_shared<MockCamera>(this->state->camera_config));
+        this->state->setCamera(std::make_shared<MockCamera>(this->state->config.camera_config));
     }
 }
 

src/cv/aggregator.cpp

@@ -4,7 +4,7 @@
 #include "utilities/logging.hpp"
 #include "utilities/lockptr.hpp"
 
-CVAggregator::CVAggregator(Pipeline p): pipeline{p} {
+CVAggregator::CVAggregator(Pipeline&& p): pipeline{p} {
     this->num_worker_threads = 0;
     this->results = std::make_shared<CVResults>();
     this->results->matches[BottleDropIndex::A] = -1;