diff --git a/configs/dev.json b/configs/dev.json index 9128d4e0..396ea16d 100644 --- a/configs/dev.json +++ b/configs/dev.json @@ -18,6 +18,7 @@ }, "pathing": { "laps": 2, + "upload_distance_buffer_m": 50.0, "dubins": { "turning_radius": 30.0, "point_separation": 20.0 @@ -54,9 +55,12 @@ }, "cv": { "yolo_model_dir": "/workspaces/obcpp/models/yolo-wittner-v2.onnx", - "detection_threshold": 0.35, + "detection_threshold": 0.0001, "input_width": 1024, - "input_height": 1024 + "input_height": 1024, + "sample_every_n_images": 10, + "image_listener_poll_interval_ms": 500, + "image_listener_settle_time_ms": 200 }, "camera": { "_comment": "See CameraConfig struct in datatypes.hpp for detailed explanations", diff --git a/configs/jetson.json b/configs/jetson.json index 17bd1675..9cbcdbad 100644 --- a/configs/jetson.json +++ b/configs/jetson.json @@ -18,6 +18,7 @@ }, "pathing": { "laps": 10, + "upload_distance_buffer_m": 50.0, "dubins": { "turning_radius": 5.0, "point_separation": 20.0 @@ -56,7 +57,10 @@ "yolo_model_dir": "/obcpp/models/yolo-wittner-v2.onnx", "detection_threshold": 0.35, "input_width": 1024, - "input_height": 1024 + "input_height": 1024, + "sample_every_n_images": 10, + "image_listener_poll_interval_ms": 500, + "image_listener_settle_time_ms": 200 }, "camera": { "_comment": "See CameraConfig struct in datatypes.hpp for detailed explanations", diff --git a/include/core/mission_state.hpp b/include/core/mission_state.hpp index 10d92eb1..1e6eb881 100644 --- a/include/core/mission_state.hpp +++ b/include/core/mission_state.hpp @@ -9,6 +9,7 @@ #include #include #include +#include #include #include #include @@ -48,6 +49,9 @@ class MissionState { void setInitPath(const MissionPath& init_path); MissionPath getInitPath(); + void setNextWaypointPath(const MissionPath& next_waypoint_path); + MissionPath getNextWaypointPath(); + void setCoveragePath(const MissionPath& coverage_path); MissionPath getCoveragePath(); @@ -98,6 +102,13 @@ class MissionState { std::shared_ptr getCV(); void setCV(std::shared_ptr cv); + + struct MatchedResults { + std::unordered_map matched_airdrop; + }; + + LockPtr getMatchedResults(); + enum class CVStatus { None = 0, Validated = 1, @@ -139,6 +150,8 @@ class MissionState { std::mutex init_path_mut; // for reading/writing the initial path MissionPath init_path; + std::mutex next_waypoint_path_mut; // for reading/writing the next waypoint path + MissionPath next_waypoint_path; std::mutex coverage_path_mut; // for reading/writing the coverage path MissionPath coverage_path; std::mutex airdrop_path_mut; @@ -154,6 +167,9 @@ class MissionState { std::shared_ptr airdrop; std::shared_ptr cv; + std::mutex matched_results_mut; + std::shared_ptr matched_results; + std::mutex cv_status_mut; CVStatus cv_status = CVStatus::None; diff --git a/include/cv/aggregator.hpp b/include/cv/aggregator.hpp index a11d146f..34e8eb5b 100644 --- a/include/cv/aggregator.hpp +++ b/include/cv/aggregator.hpp @@ -3,14 +3,16 @@ #include #include +#include #include #include #include #include #include +#include #include +#include #include -#include #include #include "cv/pipeline.hpp" @@ -30,27 +32,28 @@ 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(Pipeline&& p, const std::string& image_dir, int sample_every_n_images, + int image_listener_poll_interval_ms, int image_listener_settle_time_ms); ~CVAggregator(); // Spawn a thread to run the pipeline on the given imageData void runPipeline(const ImageData& image); + // Watch a camera save directory and submit every nth saved image to the pipeline + void startListening(const std::string& image_dir, int sample_every_n_images, + int image_listener_poll_interval_ms, int image_listener_settle_time_ms); + + // Stop watching the camera save directory + void stopListening(); + // Stop accepting work, discard queued images, and wait for active workers to finish void terminate(); // 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(); @@ -65,22 +68,26 @@ class CVAggregator { std::shared_ptr> cv_record; void worker(ImageData image, int thread_num); + void listenForImages(std::filesystem::path image_dir, int sample_every_n_images, + int image_listener_poll_interval_ms, int image_listener_settle_time_ms); + std::optional loadImageData(const std::filesystem::path& image_path) const; + std::optional loadTelemetry( + const std::filesystem::path& telemetry_path) const; Pipeline pipeline; std::mutex mut; std::atomic num_worker_threads; std::atomic accepting_images; + std::atomic listening_images; std::vector worker_threads; + std::thread listener_thread; // 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/network/mavlink.hpp b/include/network/mavlink.hpp index 814ef863..6f072db5 100644 --- a/include/network/mavlink.hpp +++ b/include/network/mavlink.hpp @@ -83,6 +83,7 @@ class MavlinkClient { mavsdk::Telemetry::RcStatus get_conn_status(); bool armAndHover(std::shared_ptr state); bool startMission(); + bool clearMission(); /* * Triggers a relay on the ArduPilot @@ -92,10 +93,9 @@ class MavlinkClient { */ bool triggerRelay(int relay_number, bool state); + // Safety Functions void KILL_THE_PLANE_DO_NOT_CALL_THIS_ACCIDENTALLY(); - - // rtl - void rtl(); + void returnToLaunch(); private: mavsdk::Mavsdk mavsdk; diff --git a/include/pathing/static.hpp b/include/pathing/static.hpp index da9bb561..b8a615b9 100644 --- a/include/pathing/static.hpp +++ b/include/pathing/static.hpp @@ -302,11 +302,27 @@ class AirdropApproachPathing { */ RRTPoint getCurrentLoc(std::shared_ptr state); -MissionPath generateInitialPath(std::shared_ptr state); +/** + * Calculates the approximate angle of exit out of a waypoint path + * + * @param path ==> the waypoint path + * @param state ==> mission state + * @return angle of exit (+x-axis is 0 deg, ccw is positive) + */ +double calculateFinalAngle( + const MissionPath& path, + const std::optional>& cartesianConverter); + +std::vector generateInitialPath(std::shared_ptr state); + +std::vector +generateNextWaypointPath(std::shared_ptr state, double start_angle); -MissionPath generateSearchPath(std::shared_ptr state); +std::vector +generateSearchPath(std::shared_ptr state, double start_angle); -MissionPath generateAirdropApproach(std::shared_ptr state, const GPSCoord &goal); +std::vector +generateAirdropApproach(std::shared_ptr state, const GPSCoord &goal); std::pair estimateAreaCoveredAndPathLength(const std::vector &goals, const Environment &airspace); diff --git a/include/ticks/airdrop_approach.hpp b/include/ticks/airdrop_approach.hpp index 0261f9bc..66351a8f 100644 --- a/include/ticks/airdrop_approach.hpp +++ b/include/ticks/airdrop_approach.hpp @@ -20,6 +20,9 @@ class AirdropApproachTick : public Tick { std::chrono::milliseconds getWait() const override; Tick* tick() override; + + private: + bool mission_started; }; bool triggerAirdrop(std::shared_ptr mav, airdrop_t airdrop_index); diff --git a/include/ticks/path_gen.hpp b/include/ticks/path_gen.hpp index 11a3489c..76e0537d 100644 --- a/include/ticks/path_gen.hpp +++ b/include/ticks/path_gen.hpp @@ -27,8 +27,7 @@ class PathGenTick : public Tick { void init() override; Tick* tick() override; private: - std::future init_path; - std::future coverage_path; + std::future paths_future; void startPathGeneration(); }; diff --git a/include/utilities/obc_config.hpp b/include/utilities/obc_config.hpp index c58169ed..78480ab7 100644 --- a/include/utilities/obc_config.hpp +++ b/include/utilities/obc_config.hpp @@ -40,6 +40,9 @@ struct CVConfig { float detection_threshold; int input_width; int input_height; + int sample_every_n_images; + int image_listener_poll_interval_ms; + int image_listener_settle_time_ms; std::string not_stolen_addr; uint16_t not_stolen_port; }; @@ -106,6 +109,7 @@ struct AirdropApproachConfig { struct PathingConfig { int laps; + double upload_distance_buffer_m; DubinsConfig dubins; RRTConfig rrt; AirdropCoverageConfig coverage; diff --git a/src/camera/mock.cpp b/src/camera/mock.cpp index 730c3a61..f43c405e 100644 --- a/src/camera/mock.cpp +++ b/src/camera/mock.cpp @@ -133,9 +133,12 @@ std::optional MockCamera::takePicture(const std::chrono::milliseconds return std::nullopt; } + uint64_t timestamp = std::chrono::duration_cast( + std::chrono::system_clock::now().time_since_epoch()).count(); + ImageData img_data( img, - 0, + timestamp, telemetry); return img_data; diff --git a/src/core/mission_state.cpp b/src/core/mission_state.cpp index 38e14021..5c96ab2b 100644 --- a/src/core/mission_state.cpp +++ b/src/core/mission_state.cpp @@ -16,7 +16,21 @@ #include "utilities/logging.hpp" #include "utilities/obc_config.hpp" -MissionState::MissionState(OBCConfig config) : config(config) {} +MissionState::MissionState(OBCConfig config) : config(config) { + this->matched_results = std::make_shared(); + + AirdropTarget dummy; + GPSCoord* coord_in_dummy = dummy.mutable_coordinate(); + coord_in_dummy->set_altitude(0.0); + coord_in_dummy->set_latitude(0.0); + coord_in_dummy->set_longitude(0.0); + + dummy.set_index(AirdropType::Water); + this->matched_results->matched_airdrop[AirdropType::Water] = dummy; + + dummy.set_index(AirdropType::Beacon); + this->matched_results->matched_airdrop[AirdropType::Beacon] = dummy; +} // Need to explicitly define now that Tick is no longer an incomplete class // See: @@ -79,6 +93,16 @@ MissionPath MissionState::getInitPath() { return this->init_path; } +void MissionState::setNextWaypointPath(const MissionPath& next_waypoint_path) { + Lock lock(this->next_waypoint_path_mut); + this->next_waypoint_path = next_waypoint_path; +} + +MissionPath MissionState::getNextWaypointPath() { + Lock lock(this->next_waypoint_path_mut); + return this->next_waypoint_path; +} + void MissionState::setCoveragePath(const MissionPath& coverage_path) { Lock lock(this->coverage_path_mut); this->coverage_path = coverage_path; @@ -121,6 +145,11 @@ std::shared_ptr MissionState::getCV() { return this->cv; } void MissionState::setCV(std::shared_ptr cv) { this->cv = cv; } +LockPtr MissionState::getMatchedResults() { + return LockPtr(this->matched_results, + &this->matched_results_mut); +} + MissionState::CVStatus MissionState::getCVStatus() { Lock lock(this->cv_status_mut); return this->cv_status; diff --git a/src/cv/aggregator.cpp b/src/cv/aggregator.cpp index bfce6146..429a2da6 100644 --- a/src/cv/aggregator.cpp +++ b/src/cv/aggregator.cpp @@ -1,42 +1,102 @@ #include "cv/aggregator.hpp" +#include +#include +#include #include +#include +#include +#include +#include +#include "nlohmann/json.hpp" #include "utilities/constants.hpp" #include "utilities/lockptr.hpp" #include "utilities/locks.hpp" #include "utilities/logging.hpp" -CVAggregator::CVAggregator(Pipeline&& p) : pipeline(std::move(p)) { +namespace { + +bool isSupportedImageFile(const std::filesystem::path& path) { + std::string ext = path.extension().string(); + std::transform(ext.begin(), ext.end(), ext.begin(), + [](unsigned char c) { return static_cast(std::tolower(c)); }); + return ext == ".jpg" || ext == ".jpeg" || ext == ".png"; +} + +std::vector listImageFiles(const std::filesystem::path& image_dir) { + std::vector image_paths; + + for (const auto& entry : std::filesystem::directory_iterator(image_dir)) { + if (entry.is_regular_file() && isSupportedImageFile(entry.path())) { + image_paths.push_back(entry.path()); + } + } + + return image_paths; +} + +} // namespace + +CVAggregator::CVAggregator( + Pipeline&& p, const std::string& image_dir, int sample_every_n_images, + int image_listener_poll_interval_ms, int image_listener_settle_time_ms) + : pipeline(std::move(p)) { this->num_worker_threads.store(0); this->accepting_images.store(true); + this->listening_images.store(false); this->results = std::make_shared(); - this->matched_results = std::make_shared(); this->cv_record = 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); + this->startListening(image_dir, sample_every_n_images, image_listener_poll_interval_ms, + image_listener_settle_time_ms); +} - // Now assign copies, each with its specific index - dummy.set_index(AirdropType::Water); - this->matched_results->matched_airdrop[AirdropType::Water] = dummy; +CVAggregator::~CVAggregator() { this->terminate(); } + +void CVAggregator::startListening( + const std::string& image_dir, int sample_every_n_images, + int image_listener_poll_interval_ms, int image_listener_settle_time_ms) { + if (sample_every_n_images <= 0) { + LOG_F(FATAL, "cv.sample_every_n_images must be greater than 0."); + } + + if (image_listener_poll_interval_ms <= 0) { + LOG_F(FATAL, "cv.image_listener_poll_interval_ms must be greater than 0."); + } + + if (image_listener_settle_time_ms <= 0) { + LOG_F(FATAL, "cv.image_listener_settle_time_ms must be greater than 0."); + } - dummy.set_index(AirdropType::Beacon); - this->matched_results->matched_airdrop[AirdropType::Beacon] = dummy; + std::filesystem::path image_path = image_dir; + if (!std::filesystem::is_directory(image_path)) { + LOG_F(FATAL, "CV image directory does not exist: %s", image_path.string().c_str()); + } + + if (this->listening_images.exchange(true)) { + LOG_F(WARNING, "CVAggregator is already listening for camera images."); + return; + } + + this->listener_thread = std::thread(&CVAggregator::listenForImages, this, image_path, + sample_every_n_images, + image_listener_poll_interval_ms, + image_listener_settle_time_ms); } -CVAggregator::~CVAggregator() { this->terminate(); } +void CVAggregator::stopListening() { + if (!this->listening_images.exchange(false)) { + return; + } -LockPtr CVAggregator::getResults() { - return LockPtr(this->results, &this->mut); + if (this->listener_thread.joinable()) { + this->listener_thread.join(); + } } -LockPtr CVAggregator::getMatchedResults() { - return LockPtr(this->matched_results, &this->mut); +LockPtr CVAggregator::getResults() { + return LockPtr(this->results, &this->mut); } LockPtr> CVAggregator::getCVRecord() { @@ -81,6 +141,8 @@ void CVAggregator::runPipeline(const ImageData& image) { } void CVAggregator::terminate() { + this->stopListening(); + std::vector threads; { Lock lock(this->mut); @@ -105,6 +167,120 @@ void CVAggregator::terminate() { } } +std::optional CVAggregator::loadTelemetry( + const std::filesystem::path& telemetry_path) const { + std::ifstream telemetry_file(telemetry_path); + if (!telemetry_file.is_open()) { + return std::nullopt; + } + + try { + nlohmann::json telemetry_json = nlohmann::json::parse(telemetry_file, nullptr, true, true); + return ImageTelemetry{ + .latitude_deg = telemetry_json.at("latitude_deg").get(), + .longitude_deg = telemetry_json.at("longitude_deg").get(), + .altitude_agl_m = telemetry_json.at("altitude_agl_m").get(), + .airspeed_m_s = telemetry_json.at("airspeed_m_s").get(), + .heading_deg = telemetry_json.at("heading_deg").get(), + .yaw_deg = telemetry_json.at("yaw_deg").get(), + .pitch_deg = telemetry_json.at("pitch_deg").get(), + .roll_deg = telemetry_json.at("roll_deg").get(), + }; + } catch (const std::exception& err) { + LOG_F(WARNING, "Failed to load image telemetry from %s: %s", + telemetry_path.string().c_str(), err.what()); + return std::nullopt; + } +} + +std::optional CVAggregator::loadImageData( + const std::filesystem::path& image_path) const { + cv::Mat image = cv::imread(image_path.string(), cv::IMREAD_COLOR); + if (image.empty()) { + LOG_F(WARNING, "Failed to load camera image %s", image_path.string().c_str()); + return std::nullopt; + } + + std::filesystem::path telemetry_path = image_path; + telemetry_path.replace_extension(".json"); + std::optional telemetry = this->loadTelemetry(telemetry_path); + if (!telemetry.has_value()) { + LOG_F(WARNING, "Skipping camera image %s because telemetry could not be loaded from %s.", + image_path.string().c_str(), telemetry_path.string().c_str()); + return std::nullopt; + } + + return ImageData{ + .DATA = std::move(image), + .TIMESTAMP = 0, + .TELEMETRY = telemetry, + }; +} + +void CVAggregator::listenForImages( + std::filesystem::path image_dir, int sample_every_n_images, + int image_listener_poll_interval_ms, int image_listener_settle_time_ms) { + loguru::set_thread_name("cv listener"); + const auto poll_interval = std::chrono::milliseconds(image_listener_poll_interval_ms); + const auto settle_time = std::chrono::milliseconds(image_listener_settle_time_ms); + + std::set seen_images; + for (const auto& image_path : listImageFiles(image_dir)) { + seen_images.insert(image_path); + } + + std::map pending_images; + std::size_t image_count = 0; + + LOG_F(INFO, + "CVAggregator listening for images in %s. Sampling every %d image(s), polling every %d " + "ms, settling for %d ms.", + image_dir.string().c_str(), sample_every_n_images, image_listener_poll_interval_ms, + image_listener_settle_time_ms); + + while (this->listening_images.load()) { + const auto now = std::chrono::steady_clock::now(); + + for (const auto& image_path : listImageFiles(image_dir)) { + if (seen_images.contains(image_path) || pending_images.contains(image_path)) { + continue; + } + pending_images.insert({image_path, now}); + } + + for (auto iter = pending_images.begin(); iter != pending_images.end();) { + if (!this->listening_images.load()) { + break; + } + + const auto& image_path = iter->first; + if (now - iter->second < settle_time) { + ++iter; + continue; + } + + std::optional image = this->loadImageData(image_path); + seen_images.insert(image_path); + iter = pending_images.erase(iter); + + if (!image.has_value()) { + continue; + } + + ++image_count; + if (image_count % static_cast(sample_every_n_images) != 0) { + continue; + } + + this->runPipeline(image.value()); + } + + std::this_thread::sleep_for(poll_interval); + } + + LOG_F(INFO, "CVAggregator image listener stopped."); +} + static std::atomic global_run_id{0}; void CVAggregator::worker(ImageData image, int thread_num) { diff --git a/src/network/gcs_routes.cpp b/src/network/gcs_routes.cpp index 32f4054a..2e30fa4e 100644 --- a/src/network/gcs_routes.cpp +++ b/src/network/gcs_routes.cpp @@ -240,15 +240,15 @@ DEF_GCS_HANDLE(Get, camera, capture) { std::optional image = cam->takePicture(1000ms, state->getMav()); - if (state->config.camera.save_images_to_file) { - image->saveToFile(state->config.camera.save_dir); - } - if (!image.has_value()) { LOG_RESPONSE(ERROR, "Failed to capture image", INTERNAL_SERVER_ERROR); return; } + if (state->config.camera.save_images_to_file) { + image->saveToFile(state->config.camera.save_dir); + } + std::optional telemetry = image->TELEMETRY; std::optional optCompressedImg = compressImg(image->DATA); cv::Mat compressed_image = optCompressedImg.has_value() ? @@ -415,7 +415,7 @@ DEF_GCS_HANDLE(Post, targets, matched) { state->getCV()->terminate(); { - LockPtr matched_results = state->getCV()->getMatchedResults(); + LockPtr matched_results = state->getMatchedResults(); if (matched_results.data == nullptr) { LOG_S(ERROR) << "lockptr is null"; @@ -425,12 +425,9 @@ DEF_GCS_HANDLE(Post, targets, matched) { AirdropTarget returned_matched_result; 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(); } } @@ -528,18 +525,12 @@ DEF_GCS_HANDLE(Get, obcstate) { } DEF_GCS_HANDLE(Post, camera, runpipeline) { + // This is only used from the shitty debug OBC page from the GCS LOG_REQUEST("POST", "/camera/runpipeline"); std::shared_ptr cam = state->getCamera(); - std::string yolo_model_dir = state->config.cv.yolo_model_dir; - LOG_F(INFO, "Instantiating CV Aggregator with the following models:"); - LOG_F(INFO, "Yolo Model: %s", yolo_model_dir.c_str()); - - // Make a CVAggregator instance and set it in the state - state->setCV(std::make_shared(Pipeline(PipelineParams( - yolo_model_dir, state->config.cv.detection_threshold, state->config.cv.input_width, - state->config.cv.input_height)))); + std::shared_ptr aggregator = state->getCV(); if (!cam->isConnected()) { LOG_F(INFO, "Camera not connected. Attempting to connect..."); @@ -556,13 +547,10 @@ DEF_GCS_HANDLE(Post, camera, runpipeline) { for (int i = 0; i < state->config.pathing.coverage.hover.pictures_per_stop; i++) { auto photo = state->getCamera()->takePicture(500ms, state->getMav()); - if (state->config.camera.save_images_to_file) { - photo->saveToFile(state->config.camera.save_dir); - } - if (photo.has_value()) { - // Run the pipeline on the photo - state->getCV()->runPipeline(photo.value()); + if (state->config.camera.save_images_to_file) { + photo->saveToFile(state->config.camera.save_dir); + } } } @@ -571,7 +559,7 @@ DEF_GCS_HANDLE(Post, camera, runpipeline) { DEF_GCS_HANDLE(Post, rtl) { LOG_REQUEST("POST", "/rtl"); - state->getMav()->rtl(); + state->getMav()->returnToLaunch(); LOG_RESPONSE(INFO, "RTL activated", OK); } diff --git a/src/network/mavlink.cpp b/src/network/mavlink.cpp index 19346af5..3dba2099 100644 --- a/src/network/mavlink.cpp +++ b/src/network/mavlink.cpp @@ -186,7 +186,7 @@ MavlinkClient::MavlinkClient(OBCConfig config) }); this->telemetry->subscribe_heading([this](mavsdk::Telemetry::Heading heading) { - VLOG_F(DEBUG, "Heading: %d", heading.heading_deg); + VLOG_F(DEBUG, "Heading: %f deg", heading.heading_deg); Lock lock(this->data_mut); this->data.heading_deg = heading.heading_deg; }); @@ -511,12 +511,27 @@ bool MavlinkClient::startMission() { return true; } +/** + * Clears the existng mission + */ +bool MavlinkClient::clearMission() { + LOG_F(INFO, "Sending clear mission command"); + auto start_result = this->mission->clear_mission(); + if (start_result != mavsdk::MissionRaw::Result::Success) { + LOG_S(ERROR) << "FAIL: Mission could not be cleared " << start_result; + return false; + } + + LOG_F(INFO, "Mission Cleared!"); + return true; +} + void MavlinkClient::KILL_THE_PLANE_DO_NOT_CALL_THIS_ACCIDENTALLY() { LOG_F(ERROR, "KILLING THE PLANE: SETTING AFS_TERMINATE TO 1"); auto result = this->param->set_param_int("AFS_TERMINATE", 1); LOG_S(ERROR) << "KILL RESULT: " << result; } -void MavlinkClient::rtl() { +void MavlinkClient::returnToLaunch() { this->action->return_to_launch(); } diff --git a/src/pathing/static.cpp b/src/pathing/static.cpp index 83cd6949..2039251d 100644 --- a/src/pathing/static.cpp +++ b/src/pathing/static.cpp @@ -21,7 +21,7 @@ #include "utilities/rng.hpp" RRT::RRT(RRTPoint start, std::vector goals, double search_radius, Polygon bounds, - const OBCConfig &config, std::vector obstacles, std::vector angles) + const OBCConfig& config, std::vector obstacles, std::vector angles) : iterations_per_waypoint(config.pathing.rrt.iterations_per_waypoint), search_radius(search_radius), rewire_radius(config.pathing.rrt.rewire_radius), @@ -34,7 +34,7 @@ RRT::RRT(RRTPoint start, std::vector goals, double search_radius, Poly } RRT::RRT(RRTPoint start, std::vector goals, double search_radius, Environment airspace, - const OBCConfig &config, std::vector angles) + const OBCConfig& config, std::vector angles) : iterations_per_waypoint(config.pathing.rrt.iterations_per_waypoint), search_radius(search_radius), rewire_radius(config.pathing.rrt.rewire_radius), @@ -96,7 +96,7 @@ bool RRT::RRTIteration(int tries, int current_goal_index) { const RRTPoint sample = generateSamplePoint(); // returns all dubins options from the tree to the sample - const std::vector, RRTOption>> &options = + const std::vector, RRTOption>>& options = tree.pathingOptions(sample, config.point_fetch_method); // returns true if the node is successfully added to the tree @@ -126,22 +126,19 @@ bool RRT::RRTIteration(int tries, int current_goal_index) { return true; } -bool RRT::epochEvaluation(std::shared_ptr goal_node, - std::shared_ptr goal_parent, +bool RRT::epochEvaluation(std::shared_ptr goal_node, std::shared_ptr goal_parent, int current_goal_index) { // If a single epoch has not been passed, mark this goal as the first // benchmark. if (goal_node == nullptr) { - goal_node = sampleToGoal(current_goal_index, - TOTAL_OPTIONS_FOR_GOAL_CONNECTION, - goal_parent); + goal_node = + sampleToGoal(current_goal_index, TOTAL_OPTIONS_FOR_GOAL_CONNECTION, goal_parent); return false; } std::shared_ptr new_parent = nullptr; - std::shared_ptr new_node = sampleToGoal(current_goal_index, - TOTAL_OPTIONS_FOR_GOAL_CONNECTION, - new_parent); + std::shared_ptr new_node = + sampleToGoal(current_goal_index, TOTAL_OPTIONS_FOR_GOAL_CONNECTION, new_parent); if (new_node == nullptr) { return false; @@ -174,7 +171,7 @@ RRT::getOptionsToGoal(int current_goal_index, int total_options) const { // Generates goal specific points based on current Waypoints and list og // Angles for (const double angle : angles) { - const XYZCoord &goal = tree.getAirspace().getGoal(current_goal_index); + const XYZCoord& goal = tree.getAirspace().getGoal(current_goal_index); goal_points.push_back(RRTPoint(goal, angle)); } @@ -188,36 +185,35 @@ RRT::getOptionsToGoal(int current_goal_index, int total_options) const { // gets all options for each of the goals, and puts them into a unified list // TODO ? maybe better for a max heap? - for (const RRTPoint &goal : goal_points) { - const std::vector, RRTOption>> &options = + for (const RRTPoint& goal : goal_points) { + const std::vector, RRTOption>>& options = // For now, we use optimal pathing tree.pathingOptions(goal, PointFetchMethod::Enum::NONE, NUMBER_OPTIONS_EACH); - for (const auto &[node, option] : options) { + for (const auto& [node, option] : options) { all_options.push_back({goal, {node, option}}); } } - std::sort(all_options.begin(), all_options.end(), [](const auto &a, const auto &b) { - auto &[a_goal, a_paths] = a; - auto &[a_node, a_option] = a_paths; - auto &[b_goal, b_paths] = b; - auto &[b_node, b_option] = b_paths; + std::sort(all_options.begin(), all_options.end(), [](const auto& a, const auto& b) { + auto& [a_goal, a_paths] = a; + auto& [a_node, a_option] = a_paths; + auto& [b_goal, b_paths] = b; + auto& [b_node, b_option] = b_paths; return a_option.length + a_node->getCost() < b_option.length + b_node->getCost(); }); return all_options; } -std::shared_ptr RRT::sampleToGoal(int current_goal_index, - int total_options, +std::shared_ptr RRT::sampleToGoal(int current_goal_index, int total_options, std::shared_ptr& parent) const { // gets all options for each of the goals - const auto &all_options = getOptionsToGoal(current_goal_index, total_options); + const auto& all_options = getOptionsToGoal(current_goal_index, total_options); // - for (const auto &[goal, pair] : all_options) { - auto &[anchor_node, option] = pair; + for (const auto& [goal, pair] : all_options) { + auto& [anchor_node, option] = pair; std::shared_ptr new_node = tree.generateNode(anchor_node, goal, option); @@ -242,8 +238,7 @@ bool RRT::connectToGoal(int current_goal_index, int total_options) { return true; } -void RRT::addNodeToTree(std::shared_ptr goal_node, - std::shared_ptr parent, +void RRT::addNodeToTree(std::shared_ptr goal_node, std::shared_ptr parent, int current_goal_index) { // add the node to the tree tree.addNode(parent, goal_node); @@ -261,7 +256,7 @@ void RRT::addNodeToTree(std::shared_ptr goal_node, double height_difference = tree.getAirspace().getGoal(current_goal_index).z - start_height; double height_increment = height_difference / local_path.size(); - for (XYZCoord &point : local_path) { + for (XYZCoord& point : local_path) { point.z = start_height; start_height += height_increment; } @@ -272,9 +267,9 @@ void RRT::addNodeToTree(std::shared_ptr goal_node, } std::shared_ptr RRT::parseOptions( - const std::vector, RRTOption>> &options, - const RRTPoint &sample) { - for (auto &[node, option] : options) { + const std::vector, RRTOption>>& options, + const RRTPoint& sample) { + for (auto& [node, option] : options) { /* * stop if * 1. the node is null @@ -301,9 +296,9 @@ std::shared_ptr RRT::parseOptions( void RRT::optimizeTree(std::shared_ptr sample) { tree.RRTStar(sample, rewire_radius); } -ForwardCoveragePathing::ForwardCoveragePathing(const RRTPoint &start, double scan_radius, +ForwardCoveragePathing::ForwardCoveragePathing(const RRTPoint& start, double scan_radius, Polygon bounds, Polygon airdrop_zone, - const OBCConfig &config, + const OBCConfig& config, std::vector obstacles) : start(start), scan_radius(scan_radius), @@ -348,7 +343,7 @@ std::vector ForwardCoveragePathing::coverageOptimal() const { // generates the endpoints for the lines (including headings) for (int i = 0; i < configs.size(); i++) { - const auto &config = configs[i]; + const auto& config = configs[i]; std::vector waypoints = airspace.getAirdropWaypoints(scan_radius, config.first, config.second); @@ -385,7 +380,7 @@ std::vector ForwardCoveragePathing::coverageOptimal() const { } std::vector ForwardCoveragePathing::generatePath( - const std::vector &dubins_options, const std::vector &waypoints) const { + const std::vector& dubins_options, const std::vector& waypoints) const { std::vector path; // height adjustement @@ -397,7 +392,7 @@ std::vector ForwardCoveragePathing::generatePath( double height_increment = height_difference / path_coordinates.size(); - for (XYZCoord &coord : path_coordinates) { + for (XYZCoord& coord : path_coordinates) { coord.z = height; height += height_increment; } @@ -410,7 +405,7 @@ std::vector ForwardCoveragePathing::generatePath( dubins.generatePoints(waypoints[i], waypoints[i + 1], dubins_options[i].dubins_path, dubins_options[i].has_straight); - for (XYZCoord &coord : path_coordinates) { + for (XYZCoord& coord : path_coordinates) { coord.z = config.altitude_m; } @@ -426,9 +421,9 @@ HoverCoveragePathing::HoverCoveragePathing(std::shared_ptr state) state{state} {} // Function to calculate the center of the polygon -XYZCoord calculateCenter(const std::vector &polygon) { +XYZCoord calculateCenter(const std::vector& polygon) { XYZCoord center(0.0, 0.0, 0.0); - for (const auto &point : polygon) { + for (const auto& point : polygon) { center.x += point.x; center.y += point.y; } @@ -438,24 +433,24 @@ XYZCoord calculateCenter(const std::vector &polygon) { } // Function to scale the polygon -void scalePolygon(std::vector &polygon, double scaleFactor) { +void scalePolygon(std::vector& polygon, double scaleFactor) { // Step 1: Calculate the center of the polygon XYZCoord center = calculateCenter(polygon); // Step 2: Translate the polygon to the origin - for (auto &point : polygon) { + for (auto& point : polygon) { point.x -= center.x; point.y -= center.y; } // Step 3: Scale the polygon - for (auto &point : polygon) { + for (auto& point : polygon) { point.x *= scaleFactor; point.y *= scaleFactor; } // Step 4: Translate the polygon back to its original center - for (auto &point : polygon) { + for (auto& point : polygon) { point.x += center.x; point.y += center.y; } @@ -508,9 +503,9 @@ std::vector HoverCoveragePathing::run() { return hover_points; } -AirdropApproachPathing::AirdropApproachPathing(const RRTPoint &start, const XYZCoord &goal, +AirdropApproachPathing::AirdropApproachPathing(const RRTPoint& start, const XYZCoord& goal, XYZCoord wind, Polygon bounds, - const OBCConfig &config, + const OBCConfig& config, std::vector obstacles) : start(start), goal(goal), @@ -552,7 +547,7 @@ RRTPoint AirdropApproachPathing::getDropLocation() const { return RRTPoint(drop_location, angle); } -std::vector> generateGoalListDeviations(const std::vector &goals, +std::vector> generateGoalListDeviations(const std::vector& goals, XYZCoord deviation_point) { std::vector> goal_list_deviations; for (int i = 0; i < goals.size() + 1; i++) { @@ -564,8 +559,8 @@ std::vector> generateGoalListDeviations(const std::vector< return goal_list_deviations; } -std::vector> generateRankedNewGoalsList(const std::vector &goals, - const Environment &mapping_bounds) { +std::vector> generateRankedNewGoalsList(const std::vector& goals, + const Environment& mapping_bounds) { // generate deviation points randomly in the mapping region std::vector deviation_points; for (int i = 0; i < 200; i++) { @@ -574,7 +569,7 @@ std::vector> generateRankedNewGoalsList(const std::vector< // each deviation point can be inserted between any two goals std::vector> new_goals_list; - for (const XYZCoord &deviation_point : deviation_points) { + for (const XYZCoord& deviation_point : deviation_points) { std::vector> goal_list_deviations = generateGoalListDeviations(goals, deviation_point); new_goals_list.insert(new_goals_list.end(), goal_list_deviations.begin(), @@ -583,7 +578,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) { + for (const std::vector& new_goals : new_goals_list) { area_length_pairs.push_back(mapping_bounds.estimateAreaCoveredAndPathLength(new_goals)); } @@ -595,11 +590,11 @@ std::vector> generateRankedNewGoalsList(const std::vector< } std::sort(ranked_new_goals_list.begin(), ranked_new_goals_list.end(), - [](const auto &a, const auto &b) { return a.first > b.first; }); + [](const auto& a, const auto& b) { return a.first > b.first; }); // return the ranked list of new goals lists std::vector> ranked_goals; - for (const auto &pair : ranked_new_goals_list) { + for (const auto& pair : ranked_new_goals_list) { ranked_goals.push_back(pair.second); } @@ -619,7 +614,22 @@ RRTPoint getCurrentLoc(std::shared_ptr state) { return RRTPoint(start_xyz, start_angle); } -MissionPath generateInitialPath(std::shared_ptr state) { +double calculateFinalAngle( + const MissionPath& path, + const std::optional>& cartesianConverter) { + const auto& coords = path.get(); + if (coords.size() < 2) { + return 0.0; // should probably either have angle between now and point + // or use assert for force size >=2 + } + + XYZCoord pt1 = cartesianConverter->toXYZ(coords[coords.size() - 2]); + XYZCoord pt2 = cartesianConverter->toXYZ(coords[coords.size() - 1]); + + return std::atan2(pt2.y - pt1.y, pt2.x - pt1.x); +} + +std::vector generateInitialPath(std::shared_ptr state) { // first waypoint is start // the other waypoitns is the goals @@ -649,40 +659,87 @@ MissionPath generateInitialPath(std::shared_ptr state) { std::vector path = rrt.getPointsToGoal(); std::vector output_coords; - for (const XYZCoord &waypoint : path) { + for (const XYZCoord& waypoint : path) { output_coords.push_back(state->getCartesianConverter()->toLatLng(waypoint)); } - return MissionPath(MissionPath::Type::FORWARD, output_coords); + return output_coords; } -MissionPath generateSearchPath(std::shared_ptr state) { +std::vector generateNextWaypointPath(std::shared_ptr state, + double start_angle) { + if (state->mission_params.getWaypoints().size() < 1) { + loguru::set_thread_name("Static Pathing"); + LOG_F(ERROR, "Not enough waypoints to generate a path, requires >=1, num waypoints: %s", + std::to_string(state->mission_params.getWaypoints().size()).c_str()); + return {}; + } + + std::vector goals = state->mission_params.getWaypoints(); + + if (state->config.pathing.rrt.generate_deviations) { + Environment mapping_bounds(state->mission_params.getAirdropBoundary(), {}, {}, goals, {}); + goals = generateRankedNewGoalsList(goals, mapping_bounds)[0]; + } + + RRTPoint start(goals.back(), start_angle); + + // add buffer to the start point so that we dont loopty loop + double buffer_m = state->config.pathing.upload_distance_buffer_m; + if (buffer_m > 0.0) { + start.coord.x += buffer_m * std::cos(start_angle); + start.coord.y += buffer_m * std::sin(start_angle); + } + + RRT rrt(start, goals, SEARCH_RADIUS, state->mission_params.getFlightBoundary(), state->config, + {}, {}); + + rrt.run(); + + std::vector path = rrt.getPointsToGoal(); + + std::vector output_coords; + for (const XYZCoord& waypoint : path) { + output_coords.push_back(state->getCartesianConverter()->toLatLng(waypoint)); + } + + return output_coords; +} + +std::vector generateSearchPath(std::shared_ptr state, double start_angle) { std::vector gps_coords; if (state->config.pathing.coverage.method == AirdropCoverageMethod::Enum::FORWARD) { - RRTPoint start(state->mission_params.getWaypoints().back(), 0); + if (state->mission_params.getWaypoints().size() < 1) { + loguru::set_thread_name("Static Pathing"); + LOG_F(ERROR, + "Waypoint path is empty. Failed to generate search path"); + return {}; + } + RRTPoint start(state->mission_params.getWaypoints().back(), start_angle); + double scan_radius = state->config.pathing.coverage.camera_vision_m; ForwardCoveragePathing pathing(start, scan_radius, state->mission_params.getFlightBoundary(), state->mission_params.getAirdropBoundary(), state->config); - for (const auto &coord : pathing.run()) { + for (const auto& coord : pathing.run()) { gps_coords.push_back(state->getCartesianConverter()->toLatLng(coord)); } - return MissionPath(MissionPath::Type::FORWARD, gps_coords); + return gps_coords; } else { // hover HoverCoveragePathing pathing(state); - for (const auto &coord : pathing.run()) { + 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); + return gps_coords; } } -MissionPath generateAirdropApproach(std::shared_ptr state, const GPSCoord &goal) { +std::vector generateAirdropApproach(std::shared_ptr state, + const GPSCoord& goal) { std::shared_ptr mav = state->getMav(); /* Note: this function was neutered right before we attempted to fly at the 2024 competition @@ -709,7 +766,7 @@ MissionPath generateAirdropApproach(std::shared_ptr state, const G std::vector gps_path; // XYZCoord pt = state->getCartesianConverter().value().toXYZ(goal); - for (const XYZCoord &wpt : xyz_path) { + for (const XYZCoord& wpt : xyz_path) { gps_path.push_back(state->getCartesianConverter().value().toLatLng(wpt)); } @@ -725,5 +782,5 @@ MissionPath generateAirdropApproach(std::shared_ptr state, const G // gps_path.push_back(goal); // gps_path.push_back(goal); - return MissionPath(MissionPath::Type::FORWARD, gps_path); + return gps_path; } diff --git a/src/ticks/airdrop_approach.cpp b/src/ticks/airdrop_approach.cpp index 5ca043e2..ef5462ed 100644 --- a/src/ticks/airdrop_approach.cpp +++ b/src/ticks/airdrop_approach.cpp @@ -17,11 +17,14 @@ AirdropApproachTick::AirdropApproachTick(std::shared_ptr state) - : Tick(state, TickID::AirdropApproach) {} + : Tick(state, TickID::AirdropApproach), mission_started(false) {} void AirdropApproachTick::init() { LOG_F(INFO, "start mission airdrop"); - this->state->getMav()->startMission(); + while (!this->mission_started) { + this->mission_started = this->state->getMav()->startMission(); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } } std::chrono::milliseconds AirdropApproachTick::getWait() const { @@ -62,17 +65,15 @@ Tick* AirdropApproachTick::tick() { 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 + // Trigger the airdrop 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 AirdropType when calling markAirdropAsDropped - // state->markAirdropAsDropped( - // static_cast(state->next_airdrop_to_drop.value() - 1)); + // Mark as dropped so AirdropPrep goes to the next + // target instead of re-routing to the same one. + state->markAirdropAsDropped( + static_cast(state->next_airdrop_to_drop.value())); + // Clear so we don't re-trigger on subsequent ticks before isMissionFinished. + state->next_airdrop_to_drop = std::nullopt; } else { LOG_F(ERROR, "Cannot drop bottle because no bottle to drop"); diff --git a/src/ticks/airdrop_prep.cpp b/src/ticks/airdrop_prep.cpp index fb58dc26..6f72287a 100644 --- a/src/ticks/airdrop_prep.cpp +++ b/src/ticks/airdrop_prep.cpp @@ -26,7 +26,7 @@ Tick* AirdropPrepTick::tick() { return new ManualLandingTick(state, nullptr); } - LockPtr results = state->getCV()->getMatchedResults(); + LockPtr results = state->getMatchedResults(); for (int i = AirdropType::Water; i <= AirdropType::Beacon; i++) { if (dropped_airdrops.contains(static_cast(i))) { @@ -46,8 +46,6 @@ Tick* AirdropPrepTick::tick() { break; } - 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. @@ -67,9 +65,10 @@ Tick* AirdropPrepTick::tick() { target.coordinate().latitude(), target.coordinate().longitude(), target.coordinate().altitude()); - state->setAirdropPath(generateAirdropApproach(state, target.coordinate())); + state->setAirdropPath(MissionPath(MissionPath::Type::FORWARD, + generateAirdropApproach(state, target.coordinate()))); - LOG_F(INFO, "Generated approach path"); + LOG_F(INFO, "Generated approach path: size: %ld", state->getAirdropPath().get().size()); state->next_airdrop_to_drop = static_cast(next_airdrop); diff --git a/src/ticks/fly_search.cpp b/src/ticks/fly_search.cpp index 1d733d3c..3d529677 100644 --- a/src/ticks/fly_search.cpp +++ b/src/ticks/fly_search.cpp @@ -2,6 +2,7 @@ #include #include +#include #include "pathing/environment.hpp" #include "ticks/airdrop_prep.hpp" @@ -28,17 +29,9 @@ void FlySearchTick::init() { this->airdrop_boundary = this->state->mission_params.getAirdropBoundary(); this->last_photo_time = getUnixTime_ms(); - // note: I didn't get around to testing if 1 would be a better value than 0 - // to see if the mission start can be forced. - if (!this->state->getMav()->setMissionItem(1)) { - LOG_F(ERROR, "Failed to reset Mission"); - } - - this->mission_started = this->state->getMav()->startMission(); - - // I have another one here because idk how startmIssion behaves exactly - if (!this->state->getMav()->setMissionItem(1)) { - LOG_F(ERROR, "Failed to reset Mission"); + while (!this->mission_started) { + this->mission_started = this->state->getMav()->startMission(); + std::this_thread::sleep_for(100ms); } LOG_F(INFO, "Total Waypoint #: %zu", this->state->getMav()->totalWaypoints()); @@ -51,11 +44,6 @@ Tick* FlySearchTick::tick() { return new AirdropPrepTick(this->state); } - if (!this->mission_started) { - this->mission_started = this->state->getMav()->startMission(); - return nullptr; - } - bool isMissionFinished = state->getMav()->isMissionFinished(); if (isMissionFinished) { @@ -74,15 +62,12 @@ Tick* FlySearchTick::tick() { LOG_F(INFO, "FlySearch Area reached (%zu, %d)", this->curr_mission_item, curr_waypoint); for (int i = 0; i < this->state->config.pathing.coverage.hover.pictures_per_stop; i++) { auto photo = this->state->getCamera()->takePicture(500ms, this->state->getMav()); - if (state->config.camera.save_images_to_file) { - photo->saveToFile(state->config.camera.save_dir); - } - if (photo.has_value()) { // Update the last photo time this->last_photo_time = getUnixTime_ms(); - // Run the pipeline on the photo - this->state->getCV()->runPipeline(photo.value()); + if (state->config.camera.save_images_to_file) { + photo->saveToFile(state->config.camera.save_dir); + } } } this->curr_mission_item = curr_waypoint; diff --git a/src/ticks/fly_waypoints.cpp b/src/ticks/fly_waypoints.cpp index ed8250b2..a3b60405 100644 --- a/src/ticks/fly_waypoints.cpp +++ b/src/ticks/fly_waypoints.cpp @@ -3,6 +3,7 @@ #include #include #include +#include #include "pathing/static.hpp" #include "ticks/fly_search.hpp" @@ -18,22 +19,12 @@ FlyWaypointsTick::FlyWaypointsTick(std::shared_ptr state, Tick* ne : Tick(state, TickID::FlyWaypoints), next_tick(next_tick) {} void FlyWaypointsTick::init() { - // note: I didn't get around to testing if 1 would be a better value than 0 - // to see if the mission start can be forced. - if (!this->state->getMav()->setMissionItem(1)) { - LOG_F(ERROR, "Failed to reset Mission"); + while (!this->mission_started) { + this->mission_started = this->state->getMav()->startMission(); + std::this_thread::sleep_for(100ms); } - - this->mission_started = this->state->getMav()->startMission(); - state->getCamera()->startStreaming(); - this->last_photo_time = getUnixTime_ms(); - - // I have another one here because idk how startmIssion behaves exactly - if (!this->state->getMav()->setMissionItem(1)) { - LOG_F(ERROR, "Failed to reset Mission"); - } - state->decrementLapsRemaining(); + LOG_F(INFO, "Started FlyWaypointsTick, Laps Remaining: %d", state->getLapsRemaining()); } @@ -57,15 +48,12 @@ Tick* FlyWaypointsTick::tick() { auto now = getUnixTime_ms(); if ((now - this->last_photo_time) >= 300ms) { auto photo = this->state->getCamera()->takePicture(100ms, this->state->getMav()); - if (state->config.camera.save_images_to_file) { - photo->saveToFile(state->config.camera.save_dir); - } - if (photo.has_value()) { // Update the last photo time this->last_photo_time = getUnixTime_ms(); - // Run the pipeline on the photo - this->state->getCV()->runPipeline(photo.value()); + if (state->config.camera.save_images_to_file) { + photo->saveToFile(state->config.camera.save_dir); + } } } } else { @@ -80,32 +68,10 @@ Tick* FlyWaypointsTick::tick() { return nullptr; } - if (state->getLapsRemaining() > 0) { - // regenerate path - std::future init_path; - init_path = std::async(std::launch::async, generateInitialPath, this->state); - auto init_status = init_path.wait_for(std::chrono::milliseconds(0)); - int count_ms = 2500; - const int wait_time_ms = 100; - - while (init_status != std::future_status::ready && count_ms > 0) { - LOG_F(WARNING, "Waiting for path to be generated..."); - std::this_thread::sleep_for(std::chrono::milliseconds(wait_time_ms)); - init_status = init_path.wait_for(std::chrono::milliseconds(0)); - count_ms -= wait_time_ms; - } - - if (count_ms <= 0) { - LOG_F(ERROR, "Path generation took too long. Trying Again..."); - return nullptr; - } - - state->setInitPath(init_path.get()); - return new MavUploadTick( this->state, new FlyWaypointsTick(this->state, new FlySearchTick(this->state)), - state->getInitPath(), false); + state->getNextWaypointPath(), false); } return new MavUploadTick( diff --git a/src/ticks/mav_upload.cpp b/src/ticks/mav_upload.cpp index 24e18518..2ecef4ab 100644 --- a/src/ticks/mav_upload.cpp +++ b/src/ticks/mav_upload.cpp @@ -30,6 +30,7 @@ void MavUploadTick::init() { this->state, upload_geofence, waypoints); + this->state->getMav()->setMissionItem(0); } Tick* MavUploadTick::tick() { diff --git a/src/ticks/mission_prep.cpp b/src/ticks/mission_prep.cpp index 3ffd6ef4..d85a2f69 100644 --- a/src/ticks/mission_prep.cpp +++ b/src/ticks/mission_prep.cpp @@ -36,9 +36,13 @@ Tick* MissionPrepTick::tick() { 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( + auto aggregator = std::make_shared(Pipeline(PipelineParams( yolo_model_dir, this->state->config.cv.detection_threshold, - this->state->config.cv.input_width, this->state->config.cv.input_height)))); + this->state->config.cv.input_width, this->state->config.cv.input_height)), + this->state->config.camera.save_dir, this->state->config.cv.sample_every_n_images, + this->state->config.cv.image_listener_poll_interval_ms, + this->state->config.cv.image_listener_settle_time_ms); + this->state->setCV(aggregator); this->state->setMappingIsDone(false); return new PathGenTick(this->state); diff --git a/src/ticks/path_gen.cpp b/src/ticks/path_gen.cpp index fe690df0..eb792507 100644 --- a/src/ticks/path_gen.cpp +++ b/src/ticks/path_gen.cpp @@ -18,17 +18,14 @@ PathGenTick::PathGenTick(std::shared_ptr state) : Tick(state, Tick std::chrono::milliseconds PathGenTick::getWait() const { return PATH_GEN_TICK_WAIT; } void PathGenTick::init() { + assert(this->state->getCartesianConverter().has_value()); startPathGeneration(); } Tick* PathGenTick::tick() { - auto init_status = this->init_path.wait_for(0ms); - auto search_status = this->coverage_path.wait_for(0ms); - if (init_status == std::future_status::ready && - search_status == std::future_status::ready) { + auto status = this->paths_future.wait_for(0ms); + if (status == std::future_status::ready) { LOG_F(INFO, "Initial and Coverage paths generated"); - state->setInitPath(this->init_path.get()); - state->setCoveragePath(this->coverage_path.get()); return new PathValidateTick(this->state); } @@ -36,6 +33,36 @@ Tick* PathGenTick::tick() { } void PathGenTick::startPathGeneration() { - this->init_path = std::async(std::launch::async, generateInitialPath, this->state); - this->coverage_path = std::async(std::launch::async, generateSearchPath, this->state); + this->paths_future = std::async(std::launch::async, [this]() { + const int NUM_WAYPOINTS_TO_REMOVE = + std::floor(this->state->config.pathing.upload_distance_buffer_m / + this->state->config.pathing.dubins.point_separation); + + std::vector init_gps = generateInitialPath(this->state); + MissionPath init = MissionPath(MissionPath::Type::FORWARD, init_gps); + double angle1 = calculateFinalAngle(init, this->state->getCartesianConverter()); + + std::vector next_gps = generateNextWaypointPath(this->state, angle1); + assert(next_gps.size() >= NUM_WAYPOINTS_TO_REMOVE); + next_gps.erase(next_gps.begin(), next_gps.begin() + NUM_WAYPOINTS_TO_REMOVE);; + MissionPath next = MissionPath(MissionPath::Type::FORWARD, next_gps); + double angle2 = calculateFinalAngle(next, this->state->getCartesianConverter()); + + + std::vector coverage_gps = generateSearchPath(this->state, angle2); + assert(coverage_gps.size() >= NUM_WAYPOINTS_TO_REMOVE); + coverage_gps.erase(coverage_gps.begin(), coverage_gps.begin() + NUM_WAYPOINTS_TO_REMOVE);; + + MissionPath coverage; + if (this->state->config.pathing.coverage.method == AirdropCoverageMethod::Enum::FORWARD) { + coverage = MissionPath(MissionPath::Type::FORWARD, coverage_gps); + } else { + coverage = MissionPath(MissionPath::Type::HOVER, coverage_gps, + this->state->config.pathing.coverage.hover.hover_time_s); + } + + this->state->setInitPath(init); + this->state->setNextWaypointPath(next); + this->state->setCoveragePath(coverage); + }); } diff --git a/src/utilities/obc_config.cpp b/src/utilities/obc_config.cpp index df604da2..4f1cabbf 100644 --- a/src/utilities/obc_config.cpp +++ b/src/utilities/obc_config.cpp @@ -60,6 +60,7 @@ OBCConfig::OBCConfig(int argc, char* argv[]) { SET_CONFIG_OPT(pathing, laps); SET_CONFIG_OPT(pathing, rrt, iterations_per_waypoint); + SET_CONFIG_OPT(pathing, upload_distance_buffer_m); SET_CONFIG_OPT(pathing, rrt, rewire_radius); SET_CONFIG_OPT(pathing, rrt, optimize); SET_CONFIG_OPT(pathing, rrt, generate_deviations); @@ -82,6 +83,9 @@ OBCConfig::OBCConfig(int argc, char* argv[]) { SET_CONFIG_OPT(cv, detection_threshold); SET_CONFIG_OPT(cv, input_width); SET_CONFIG_OPT(cv, input_height); + SET_CONFIG_OPT(cv, sample_every_n_images); + SET_CONFIG_OPT(cv, image_listener_poll_interval_ms); + SET_CONFIG_OPT(cv, image_listener_settle_time_ms); SET_CONFIG_OPT_VARIANT(AirdropDropMethod, pathing, approach, drop_method); SET_CONFIG_OPT(pathing, approach, drop_angle_rad); SET_CONFIG_OPT(pathing, approach, drop_altitude_m); diff --git a/tests/integration/cv_aggregator.cpp b/tests/integration/cv_aggregator.cpp index e02c7a35..5a7f4a7b 100644 --- a/tests/integration/cv_aggregator.cpp +++ b/tests/integration/cv_aggregator.cpp @@ -42,7 +42,7 @@ int main() { Pipeline pipeline(params); // Build the aggregator by transferring ownership of the pipeline - CVAggregator aggregator(std::move(pipeline)); + CVAggregator aggregator(std::move(pipeline), imagesDirectory, 1, 500, 500); // Create a mock telemetry object (using the same telemetry for simplicity) ImageTelemetry mockTelemetry{38.31568, 76.55006, 75, 20, 100, 5, 3}; diff --git a/tests/unit/modify_runs.cpp b/tests/unit/modify_runs.cpp index 57df875b..ce594dca 100644 --- a/tests/unit/modify_runs.cpp +++ b/tests/unit/modify_runs.cpp @@ -1,4 +1,6 @@ #include +#include + #include "cv/aggregator.hpp" TEST(ModifyRuns, OverWriteData) { @@ -9,7 +11,11 @@ TEST(ModifyRuns, OverWriteData) "", false); Pipeline pipeline(params); - CVAggregator aggregator(std::move(pipeline)); + CVAggregator aggregator(std::move(pipeline), + std::filesystem::temp_directory_path().string(), + 1, + 500, + 500); std::vector updated; { @@ -78,4 +84,4 @@ TEST(ModifyRuns, OverWriteData) ASSERT_EQ(value.coordinates().at(i).latitude(), ptr->at(value.run_id()).coordinates().at(i).latitude()); } } -} \ No newline at end of file +} diff --git a/tests/unit/pathing_test.cpp b/tests/unit/pathing_test.cpp index 1b8ce2ea..9cc999e9 100644 --- a/tests/unit/pathing_test.cpp +++ b/tests/unit/pathing_test.cpp @@ -1,6 +1,7 @@ #include #include +#include #include #include #include @@ -21,61 +22,182 @@ #include "utilities/http.hpp" #include "utilities/obc_config.hpp" -// TODO: currnetly failing because of the same issue as gcs_server_test.cpp -// Once that file is fixed this one should be trivial - -/* -#define DECLARE_HANDLER_PARAMS(STATE, REQ, RESP) \ - int argc = 2; \ - char path1[] = "bin/obcpp"; \ - char path2[] = "../../../configs/dev-config.json"; \ - char *paths[] = {path1, path2}; \ - char **paths_ptr = paths; \ - std::shared_ptr STATE = std::make_shared(OBCConfig(argc, paths_ptr)); \ - httplib::Request REQ; \ - httplib::Response RESP +#include "handler_params.hpp" +// TODO: fails fom the tick ever switching // copies over code verbatim from the gcs test, and then generates a path -TEST(StaticPathingTest, RRTTest) { - // First upload a mission so that we generate a path +// TEST(StaticPathingTest, RRTTest) { +// // First upload a mission so that we generate a path +// DECLARE_HANDLER_PARAMS(state, req, resp); +// req.body = resources::mission_json_2020; +// 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); + +// // actually new test +// // validate the path +// Environment env(state->mission_params.getFlightBoundary(), {}, {}, {}, {}); + +// std::vector path = state->getInitPath().get(); + +// for (GPSCoord &point : path) { +// const XYZCoord xyz_point = state->getCartesianConverter().value().toXYZ(point); + +// EXPECT_TRUE(env.isPointInBounds(xyz_point)); +// } + +// // PathingPlot plotter("pathing_output", state->config.getFlightBoundary(), +// // state->config.getAirdropBoundary(), state->config.getWaypoints()); + +// // std::vector path_coords; + +// // for (auto wpt : path) { +// // path_coords.push_back(state->getCartesianConverter().value().toXYZ(wpt)); +// // } + +// // plotter.addFinalPolyline(path_coords); +// // plotter.output("unit_test_path", PathOutputType::BOTH); +// } + +/** + * Checks final angles in each axis and quadrant (8 checks) + */ +TEST(StaticPathingTest, FinalAngleQuadrant) { DECLARE_HANDLER_PARAMS(state, req, resp); - req.body = resources::mission_json_2020; - state->setTick(new MissionPrepTick(state)); - GCS_HANDLE(Post, mission)(state, req, resp); + GPSProtoVec bounds; + *bounds.Add() = makeGPSCoord(0.0, 0.0, 0.0); + *bounds.Add() = makeGPSCoord(1.0, 1.0, 0.0); + CartesianConverter converter(bounds); + state->setCartesianConverter(converter); - 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); + struct TestCheck { + double dx; + double dy; + double expected_angle; + }; + + std::vector checks = { + {1.0, 0.0, 0.0}, + {0.0, 1.0, M_PI / 2.0}, + {-1.0, 0.0, M_PI}, + {0.0, -1.0, -M_PI / 2.0}, + {1.0, 1.0, M_PI / 4.0}, + {-1.0, 1.0, 3.0 * M_PI / 4.0}, + {-1.0, -1.0, -3.0 * M_PI / 4.0}, + {1.0, -1.0, -M_PI / 4.0} + }; - // actually new test - // validate the path - Environment env(state->mission_params.getFlightBoundary(), {}, {}, {}); + XYZCoord start_xyz(0.0, 0.0, 0.0); + GPSCoord start_gps = converter.toLatLng(start_xyz); - std::vector path = state->getInitPath().get(); + for (const auto& check : checks) { + XYZCoord end_xyz(check.dx, check.dy, 0.0); + GPSCoord end_gps = converter.toLatLng(end_xyz); - for (GPSCoord &point : path) { - const XYZCoord xyz_point = state->getCartesianConverter().value().toXYZ(point); + MissionPath path(MissionPath::Type::FORWARD, {start_gps, end_gps}); - EXPECT_TRUE(env.isPointInBounds(xyz_point)); + double calculated_angle = calculateFinalAngle(path, state->getCartesianConverter()); + + if (check.expected_angle == M_PI && calculated_angle < 0) { + EXPECT_NEAR(-M_PI, calculated_angle, 0.001); + } else { + EXPECT_NEAR(check.expected_angle, calculated_angle, 0.001); + } } +} + +/** + * Verify our implementation of degenrate paths + */ +TEST(StaticPathingTest, FinalAngleLessThanTwoPoints) { + DECLARE_HANDLER_PARAMS(state, req, resp); - // PathingPlot plotter("pathing_output", state->config.getFlightBoundary(), - // state->config.getAirdropBoundary(), state->config.getWaypoints()); + GPSProtoVec bounds; + *bounds.Add() = makeGPSCoord(0.0, 0.0, 0.0); + *bounds.Add() = makeGPSCoord(1.0, 1.0, 0.0); + CartesianConverter converter(bounds); + state->setCartesianConverter(converter); - // std::vector path_coords; + XYZCoord start_xyz(0.0, 0.0, 0.0); + GPSCoord start_gps = converter.toLatLng(start_xyz); - // for (auto wpt : path) { - // path_coords.push_back(state->getCartesianConverter().value().toXYZ(wpt)); - // } + MissionPath empty_path; + EXPECT_EQ(0.0, calculateFinalAngle(empty_path, state->getCartesianConverter())); - // plotter.addFinalPolyline(path_coords); - // plotter.output("unit_test_path", PathOutputType::BOTH); + MissionPath one_point_path(MissionPath::Type::FORWARD, {start_gps}); + EXPECT_EQ(0.0, calculateFinalAngle(one_point_path, state->getCartesianConverter())); } -*/ \ No newline at end of file + + +/** + * Makes sure last two points are taken + */ +TEST(StaticPathingTest, FinalAngleLastTwoPoints) { + DECLARE_HANDLER_PARAMS(state, req, resp); + + GPSProtoVec bounds; + *bounds.Add() = makeGPSCoord(0.0, 0.0, 0.0); + *bounds.Add() = makeGPSCoord(1.0, 1.0, 0.0); + CartesianConverter converter(bounds); + state->setCartesianConverter(converter); + + XYZCoord pt1_xyz(0.0, 0.0, 0.0); + XYZCoord pt2_xyz(1.0, -1.0, 0.0); + XYZCoord pt3_xyz(1.0, 1.0, 0.0); + + GPSCoord pt1 = converter.toLatLng(pt1_xyz); + GPSCoord pt2 = converter.toLatLng(pt2_xyz); + GPSCoord pt3 = converter.toLatLng(pt3_xyz); + + MissionPath path(MissionPath::Type::FORWARD, {pt1, pt2, pt3}); + + // final direction is from (1, -1) to (1, 1) -> dx=0, dy=2 -> M_PI / 2.0 + double expected_angle = M_PI / 2.0; + EXPECT_NEAR(expected_angle, calculateFinalAngle(path, state->getCartesianConverter()), 0.001); +} + +/** + * Accuracy test on Dubins + * + * @param Separation + */ +TEST(StaticPathingTest, FinalAngleDubinsPath) { + DECLARE_HANDLER_PARAMS(state, req, resp); + + GPSProtoVec bounds; + *bounds.Add() = makeGPSCoord(0.0, 0.0, 0.0); + *bounds.Add() = makeGPSCoord(1.0, 1.0, 0.0); + CartesianConverter converter(bounds); + state->setCartesianConverter(converter); + + const double EXPECTED_END_ANGLE = M_PI / 2.0; + + Dubins dubins(30.0, 30.0); + RRTPoint start_pt(XYZCoord(0.0, 0.0, 0.0), 0.0); + RRTPoint end_pt(XYZCoord(300.0, 300.0, 0.0), EXPECTED_END_ANGLE); + + // Dubins Path + std::vector dubins_xyz = dubins.dubinsPath(start_pt, end_pt); + std::vector dubins_gps; + for (const auto& xyz : dubins_xyz) { + dubins_gps.push_back(converter.toLatLng(xyz)); + } + MissionPath path(MissionPath::Type::FORWARD, dubins_gps); + + double calculated_angle = calculateFinalAngle(path, state->getCartesianConverter()); + + // Allow some tolerance due to point separation (10 degrees --> 0.174 rad) + EXPECT_NEAR(EXPECTED_END_ANGLE, calculated_angle, 0.174); +} \ No newline at end of file