From 20c169be66315de86a4c84334f520034d9aa93d6 Mon Sep 17 00:00:00 2001 From: Christopher Lee Date: Tue, 14 Apr 2026 14:51:33 -0700 Subject: [PATCH 01/17] feat: precompute all paths --- include/core/mission_state.hpp | 5 ++++ include/pathing/static.hpp | 6 ++++- include/ticks/path_gen.hpp | 3 +-- src/core/mission_state.cpp | 10 +++++++ src/pathing/static.cpp | 48 ++++++++++++++++++++++++++++++++-- src/ticks/fly_waypoints.cpp | 22 +--------------- src/ticks/path_gen.cpp | 23 ++++++++++------ 7 files changed, 83 insertions(+), 34 deletions(-) diff --git a/include/core/mission_state.hpp b/include/core/mission_state.hpp index 73f4af7d..bcf78c3e 100644 --- a/include/core/mission_state.hpp +++ b/include/core/mission_state.hpp @@ -48,6 +48,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(); @@ -130,6 +133,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; diff --git a/include/pathing/static.hpp b/include/pathing/static.hpp index da9bb561..6d8fc5b8 100644 --- a/include/pathing/static.hpp +++ b/include/pathing/static.hpp @@ -304,7 +304,11 @@ RRTPoint getCurrentLoc(std::shared_ptr state); MissionPath generateInitialPath(std::shared_ptr state); -MissionPath generateSearchPath(std::shared_ptr state); +MissionPath generateNextWaypointPath(std::shared_ptr state, double start_angle); + +MissionPath generateSearchPath(std::shared_ptr state, double start_angle); + +double calculateFinalAngle(const MissionPath& path, std::shared_ptr state); MissionPath generateAirdropApproach(std::shared_ptr state, const GPSCoord &goal); 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/src/core/mission_state.cpp b/src/core/mission_state.cpp index 5bee1d7d..33c52c02 100644 --- a/src/core/mission_state.cpp +++ b/src/core/mission_state.cpp @@ -79,6 +79,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; diff --git a/src/pathing/static.cpp b/src/pathing/static.cpp index 83cd6949..bb0d819a 100644 --- a/src/pathing/static.cpp +++ b/src/pathing/static.cpp @@ -656,10 +656,54 @@ MissionPath generateInitialPath(std::shared_ptr state) { return MissionPath(MissionPath::Type::FORWARD, output_coords); } -MissionPath generateSearchPath(std::shared_ptr state) { +MissionPath 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); + + 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 MissionPath(MissionPath::Type::FORWARD, output_coords); +} + +double calculateFinalAngle(const MissionPath& path, std::shared_ptr state) { + const auto& coords = path.get(); + if (coords.size() < 2) { + return 0.0; + } + + XYZCoord pt1 = state->getCartesianConverter()->toXYZ(coords[coords.size() - 2]); + XYZCoord pt2 = state->getCartesianConverter()->toXYZ(coords[coords.size() - 1]); + + return std::atan2(pt2.y - pt1.y, pt2.x - pt1.x); +} + +MissionPath 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); + RRTPoint start(state->mission_params.getWaypoints().back(), start_angle); double scan_radius = state->config.pathing.coverage.camera_vision_m; ForwardCoveragePathing pathing(start, scan_radius, diff --git a/src/ticks/fly_waypoints.cpp b/src/ticks/fly_waypoints.cpp index 5fc9d53d..76382fd2 100644 --- a/src/ticks/fly_waypoints.cpp +++ b/src/ticks/fly_waypoints.cpp @@ -81,31 +81,11 @@ Tick* FlyWaypointsTick::tick() { if (state->getLapsRemaining() > 1) { - // 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->decrementLapsRemaining(); - 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/path_gen.cpp b/src/ticks/path_gen.cpp index fe690df0..147e4c41 100644 --- a/src/ticks/path_gen.cpp +++ b/src/ticks/path_gen.cpp @@ -22,13 +22,9 @@ void PathGenTick::init() { } 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 +32,17 @@ 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]() { + MissionPath init = generateInitialPath(this->state); + double angle1 = calculateFinalAngle(init, this->state); + + MissionPath next = generateNextWaypointPath(this->state, angle1); + double angle2 = calculateFinalAngle(next, this->state); + + MissionPath coverage = generateSearchPath(this->state, angle2); + + this->state->setInitPath(init); + this->state->setNextWaypointPath(next); + this->state->setCoveragePath(coverage); + }); } From 09216373591567bcdc256b0ca1dff389e0951afd Mon Sep 17 00:00:00 2001 From: Christopher Lee Date: Thu, 23 Apr 2026 17:04:36 -0700 Subject: [PATCH 02/17] chore: add buffer between computed paths --- configs/dev.json | 1 + configs/jetson.json | 1 + include/pathing/static.hpp | 11 +++++++---- include/utilities/obc_config.hpp | 1 + src/network/mavlink.cpp | 2 +- src/pathing/static.cpp | 28 ++++++++++++++++++---------- src/ticks/airdrop_prep.cpp | 3 ++- src/ticks/path_gen.cpp | 26 ++++++++++++++++++++------ src/utilities/obc_config.cpp | 1 + 9 files changed, 52 insertions(+), 22 deletions(-) diff --git a/configs/dev.json b/configs/dev.json index 9128d4e0..8635cdb4 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 diff --git a/configs/jetson.json b/configs/jetson.json index 17bd1675..fa0a819a 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 diff --git a/include/pathing/static.hpp b/include/pathing/static.hpp index 6d8fc5b8..d1c2fc97 100644 --- a/include/pathing/static.hpp +++ b/include/pathing/static.hpp @@ -302,15 +302,18 @@ class AirdropApproachPathing { */ RRTPoint getCurrentLoc(std::shared_ptr state); -MissionPath generateInitialPath(std::shared_ptr state); +std::vector generateInitialPath(std::shared_ptr state); -MissionPath generateNextWaypointPath(std::shared_ptr state, double start_angle); +std::vector +generateNextWaypointPath(std::shared_ptr state, double start_angle); -MissionPath generateSearchPath(std::shared_ptr state, double start_angle); +std::vector +generateSearchPath(std::shared_ptr state, double start_angle); double calculateFinalAngle(const MissionPath& path, std::shared_ptr state); -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/utilities/obc_config.hpp b/include/utilities/obc_config.hpp index c58169ed..f986cd8f 100644 --- a/include/utilities/obc_config.hpp +++ b/include/utilities/obc_config.hpp @@ -106,6 +106,7 @@ struct AirdropApproachConfig { struct PathingConfig { int laps; + double upload_distance_buffer_m; DubinsConfig dubins; RRTConfig rrt; AirdropCoverageConfig coverage; diff --git a/src/network/mavlink.cpp b/src/network/mavlink.cpp index 0e39cb2f..1bdc0ed1 100644 --- a/src/network/mavlink.cpp +++ b/src/network/mavlink.cpp @@ -174,7 +174,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; }); diff --git a/src/pathing/static.cpp b/src/pathing/static.cpp index bb0d819a..cea4a0c7 100644 --- a/src/pathing/static.cpp +++ b/src/pathing/static.cpp @@ -619,7 +619,7 @@ RRTPoint getCurrentLoc(std::shared_ptr state) { return RRTPoint(start_xyz, start_angle); } -MissionPath generateInitialPath(std::shared_ptr state) { +std::vector generateInitialPath(std::shared_ptr state) { // first waypoint is start // the other waypoitns is the goals @@ -653,10 +653,11 @@ MissionPath generateInitialPath(std::shared_ptr state) { output_coords.push_back(state->getCartesianConverter()->toLatLng(waypoint)); } - return MissionPath(MissionPath::Type::FORWARD, output_coords); + return output_coords; } -MissionPath generateNextWaypointPath(std::shared_ptr state, double start_angle) { +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", @@ -673,6 +674,13 @@ MissionPath generateNextWaypointPath(std::shared_ptr state, double 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, {}, {}); @@ -685,7 +693,7 @@ MissionPath generateNextWaypointPath(std::shared_ptr state, double output_coords.push_back(state->getCartesianConverter()->toLatLng(waypoint)); } - return MissionPath(MissionPath::Type::FORWARD, output_coords); + return output_coords; } double calculateFinalAngle(const MissionPath& path, std::shared_ptr state) { @@ -700,7 +708,7 @@ double calculateFinalAngle(const MissionPath& path, std::shared_ptr state, double start_angle) { +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(), start_angle); @@ -714,19 +722,19 @@ MissionPath generateSearchPath(std::shared_ptr state, double start 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()) { 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 @@ -769,5 +777,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_prep.cpp b/src/ticks/airdrop_prep.cpp index fb58dc26..6c223f68 100644 --- a/src/ticks/airdrop_prep.cpp +++ b/src/ticks/airdrop_prep.cpp @@ -67,7 +67,8 @@ 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"); diff --git a/src/ticks/path_gen.cpp b/src/ticks/path_gen.cpp index 147e4c41..6e900717 100644 --- a/src/ticks/path_gen.cpp +++ b/src/ticks/path_gen.cpp @@ -33,14 +33,28 @@ Tick* PathGenTick::tick() { void PathGenTick::startPathGeneration() { this->paths_future = std::async(std::launch::async, [this]() { - MissionPath init = generateInitialPath(this->state); + std::vector init_gps = generateInitialPath(this->state); + MissionPath init = MissionPath(MissionPath::Type::FORWARD, init_gps); double angle1 = calculateFinalAngle(init, this->state); - - MissionPath next = generateNextWaypointPath(this->state, angle1); + + std::vector next_gps = generateNextWaypointPath(this->state, angle1); + MissionPath next = MissionPath(MissionPath::Type::FORWARD, next_gps); double angle2 = calculateFinalAngle(next, this->state); - - MissionPath coverage = generateSearchPath(this->state, angle2); - + + int num_waypoints_to_remove = + std::ceil(this->state->config.pathing.upload_distance_buffer_m / + this->state->config.pathing.dubins.point_separation); + std::vector coverage_gps = generateSearchPath(this->state, angle2); + 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..aeec5f6b 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); From cda0872823892cfb4c98fecc07bf59f0acdf06a4 Mon Sep 17 00:00:00 2001 From: Braxten Nguyen Date: Thu, 7 May 2026 18:45:45 -0700 Subject: [PATCH 03/17] feat; consolidate waypoints into one mission --- include/pathing/static.hpp | 2 +- src/pathing/static.cpp | 10 +++++----- src/ticks/path_gen.cpp | 19 +++++++++++-------- 3 files changed, 17 insertions(+), 14 deletions(-) diff --git a/include/pathing/static.hpp b/include/pathing/static.hpp index d1c2fc97..faa871b0 100644 --- a/include/pathing/static.hpp +++ b/include/pathing/static.hpp @@ -310,7 +310,7 @@ generateNextWaypointPath(std::shared_ptr state, double start_angle std::vector generateSearchPath(std::shared_ptr state, double start_angle); -double calculateFinalAngle(const MissionPath& path, std::shared_ptr state); +double calculateFinalAngle(const std::vector& path, std::shared_ptr state); std::vector generateAirdropApproach(std::shared_ptr state, const GPSCoord &goal); diff --git a/src/pathing/static.cpp b/src/pathing/static.cpp index cea4a0c7..d2e392f8 100644 --- a/src/pathing/static.cpp +++ b/src/pathing/static.cpp @@ -696,14 +696,14 @@ generateNextWaypointPath(std::shared_ptr state, double start_angle return output_coords; } -double calculateFinalAngle(const MissionPath& path, std::shared_ptr state) { - const auto& coords = path.get(); - if (coords.size() < 2) { +double calculateFinalAngle(const std::vector& path, std::shared_ptr state) { + + if (path.size() < 2) { return 0.0; } - XYZCoord pt1 = state->getCartesianConverter()->toXYZ(coords[coords.size() - 2]); - XYZCoord pt2 = state->getCartesianConverter()->toXYZ(coords[coords.size() - 1]); + XYZCoord pt1 = state->getCartesianConverter()->toXYZ(path[path.size() - 2]); + XYZCoord pt2 = state->getCartesianConverter()->toXYZ(path[path.size() - 1]); return std::atan2(pt2.y - pt1.y, pt2.x - pt1.x); } diff --git a/src/ticks/path_gen.cpp b/src/ticks/path_gen.cpp index 6e900717..14abfd45 100644 --- a/src/ticks/path_gen.cpp +++ b/src/ticks/path_gen.cpp @@ -34,12 +34,15 @@ Tick* PathGenTick::tick() { void PathGenTick::startPathGeneration() { this->paths_future = std::async(std::launch::async, [this]() { std::vector init_gps = generateInitialPath(this->state); - MissionPath init = MissionPath(MissionPath::Type::FORWARD, init_gps); - double angle1 = calculateFinalAngle(init, this->state); - + + double angle1 = calculateFinalAngle(init_gps, this->state); + std::vector next_gps = generateNextWaypointPath(this->state, angle1); - MissionPath next = MissionPath(MissionPath::Type::FORWARD, next_gps); - double angle2 = calculateFinalAngle(next, this->state); + init_gps.reserve(init_gps.size() + (state->config.pathing.laps-1) * next_gps.size()); + for (int i = 0; i < state->config.pathing.laps-1; i++) { + init_gps.insert(init_gps.end(), next_gps.begin(), next_gps.end()); + } + double angle2 = calculateFinalAngle(next_gps, this->state); int num_waypoints_to_remove = std::ceil(this->state->config.pathing.upload_distance_buffer_m / @@ -54,9 +57,9 @@ void PathGenTick::startPathGeneration() { coverage = MissionPath(MissionPath::Type::HOVER, coverage_gps, this->state->config.pathing.coverage.hover.hover_time_s); } - - this->state->setInitPath(init); - this->state->setNextWaypointPath(next); + MissionPath waypoint = MissionPath(MissionPath::Type::FORWARD, init_gps); + this->state->setInitPath(waypoint); + this->state->setNextWaypointPath(waypoint); // TO DO: DELETE NEXT WAYPOINT PATH this->state->setCoveragePath(coverage); }); } From 48d1e3976e4ce2622df153cbcf2a33e50de4521e Mon Sep 17 00:00:00 2001 From: Braxten Nguyen Date: Tue, 12 May 2026 14:11:21 -0700 Subject: [PATCH 04/17] deleted existence of next waypoint --- include/core/mission_state.hpp | 6 ++---- src/core/mission_state.cpp | 9 +-------- src/ticks/fly_waypoints.cpp | 12 ++++++------ src/ticks/path_gen.cpp | 2 +- 4 files changed, 10 insertions(+), 19 deletions(-) diff --git a/include/core/mission_state.hpp b/include/core/mission_state.hpp index bcf78c3e..4a85f2a8 100644 --- a/include/core/mission_state.hpp +++ b/include/core/mission_state.hpp @@ -48,8 +48,7 @@ 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(); @@ -133,8 +132,7 @@ 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; diff --git a/src/core/mission_state.cpp b/src/core/mission_state.cpp index 33c52c02..93e5f019 100644 --- a/src/core/mission_state.cpp +++ b/src/core/mission_state.cpp @@ -79,15 +79,8 @@ 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); diff --git a/src/ticks/fly_waypoints.cpp b/src/ticks/fly_waypoints.cpp index 34bcabbf..4fb33b31 100644 --- a/src/ticks/fly_waypoints.cpp +++ b/src/ticks/fly_waypoints.cpp @@ -81,13 +81,13 @@ Tick* FlyWaypointsTick::tick() { } - if (state->getLapsRemaining() > 1) { - state->decrementLapsRemaining(); + // if (state->getLapsRemaining() > 1) { + // state->decrementLapsRemaining(); - return new MavUploadTick( - this->state, new FlyWaypointsTick(this->state, new FlySearchTick(this->state)), - state->getNextWaypointPath(), false); - } + // return new MavUploadTick( + // this->state, new FlyWaypointsTick(this->state, new FlySearchTick(this->state)), + // state->getNextWaypointPath(), false); + // } return new MavUploadTick( this->state, new FlySearchTick(this->state), diff --git a/src/ticks/path_gen.cpp b/src/ticks/path_gen.cpp index 14abfd45..5e979475 100644 --- a/src/ticks/path_gen.cpp +++ b/src/ticks/path_gen.cpp @@ -59,7 +59,7 @@ void PathGenTick::startPathGeneration() { } MissionPath waypoint = MissionPath(MissionPath::Type::FORWARD, init_gps); this->state->setInitPath(waypoint); - this->state->setNextWaypointPath(waypoint); // TO DO: DELETE NEXT WAYPOINT PATH + this->state->setCoveragePath(coverage); }); } From d05b022460cf4abdd51b46b5d2908d28e5a71519 Mon Sep 17 00:00:00 2001 From: AskewParity Date: Tue, 12 May 2026 16:00:50 -0700 Subject: [PATCH 05/17] revert: revert past 2 commits --- include/core/mission_state.hpp | 6 ++++-- include/pathing/static.hpp | 2 +- src/core/mission_state.cpp | 9 ++++++++- src/pathing/static.cpp | 10 +++++----- src/ticks/fly_waypoints.cpp | 12 ++++++------ src/ticks/path_gen.cpp | 19 ++++++++----------- 6 files changed, 32 insertions(+), 26 deletions(-) diff --git a/include/core/mission_state.hpp b/include/core/mission_state.hpp index 4a85f2a8..bcf78c3e 100644 --- a/include/core/mission_state.hpp +++ b/include/core/mission_state.hpp @@ -48,7 +48,8 @@ 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(); @@ -132,7 +133,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; diff --git a/include/pathing/static.hpp b/include/pathing/static.hpp index faa871b0..d1c2fc97 100644 --- a/include/pathing/static.hpp +++ b/include/pathing/static.hpp @@ -310,7 +310,7 @@ generateNextWaypointPath(std::shared_ptr state, double start_angle std::vector generateSearchPath(std::shared_ptr state, double start_angle); -double calculateFinalAngle(const std::vector& path, std::shared_ptr state); +double calculateFinalAngle(const MissionPath& path, std::shared_ptr state); std::vector generateAirdropApproach(std::shared_ptr state, const GPSCoord &goal); diff --git a/src/core/mission_state.cpp b/src/core/mission_state.cpp index 93e5f019..33c52c02 100644 --- a/src/core/mission_state.cpp +++ b/src/core/mission_state.cpp @@ -79,8 +79,15 @@ 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); diff --git a/src/pathing/static.cpp b/src/pathing/static.cpp index d2e392f8..cea4a0c7 100644 --- a/src/pathing/static.cpp +++ b/src/pathing/static.cpp @@ -696,14 +696,14 @@ generateNextWaypointPath(std::shared_ptr state, double start_angle return output_coords; } -double calculateFinalAngle(const std::vector& path, std::shared_ptr state) { - - if (path.size() < 2) { +double calculateFinalAngle(const MissionPath& path, std::shared_ptr state) { + const auto& coords = path.get(); + if (coords.size() < 2) { return 0.0; } - XYZCoord pt1 = state->getCartesianConverter()->toXYZ(path[path.size() - 2]); - XYZCoord pt2 = state->getCartesianConverter()->toXYZ(path[path.size() - 1]); + XYZCoord pt1 = state->getCartesianConverter()->toXYZ(coords[coords.size() - 2]); + XYZCoord pt2 = state->getCartesianConverter()->toXYZ(coords[coords.size() - 1]); return std::atan2(pt2.y - pt1.y, pt2.x - pt1.x); } diff --git a/src/ticks/fly_waypoints.cpp b/src/ticks/fly_waypoints.cpp index 4fb33b31..34bcabbf 100644 --- a/src/ticks/fly_waypoints.cpp +++ b/src/ticks/fly_waypoints.cpp @@ -81,13 +81,13 @@ Tick* FlyWaypointsTick::tick() { } - // if (state->getLapsRemaining() > 1) { - // state->decrementLapsRemaining(); + if (state->getLapsRemaining() > 1) { + state->decrementLapsRemaining(); - // return new MavUploadTick( - // this->state, new FlyWaypointsTick(this->state, new FlySearchTick(this->state)), - // state->getNextWaypointPath(), false); - // } + return new MavUploadTick( + this->state, new FlyWaypointsTick(this->state, new FlySearchTick(this->state)), + state->getNextWaypointPath(), false); + } return new MavUploadTick( this->state, new FlySearchTick(this->state), diff --git a/src/ticks/path_gen.cpp b/src/ticks/path_gen.cpp index 5e979475..6e900717 100644 --- a/src/ticks/path_gen.cpp +++ b/src/ticks/path_gen.cpp @@ -34,15 +34,12 @@ Tick* PathGenTick::tick() { void PathGenTick::startPathGeneration() { this->paths_future = std::async(std::launch::async, [this]() { std::vector init_gps = generateInitialPath(this->state); - - double angle1 = calculateFinalAngle(init_gps, this->state); - + MissionPath init = MissionPath(MissionPath::Type::FORWARD, init_gps); + double angle1 = calculateFinalAngle(init, this->state); + std::vector next_gps = generateNextWaypointPath(this->state, angle1); - init_gps.reserve(init_gps.size() + (state->config.pathing.laps-1) * next_gps.size()); - for (int i = 0; i < state->config.pathing.laps-1; i++) { - init_gps.insert(init_gps.end(), next_gps.begin(), next_gps.end()); - } - double angle2 = calculateFinalAngle(next_gps, this->state); + MissionPath next = MissionPath(MissionPath::Type::FORWARD, next_gps); + double angle2 = calculateFinalAngle(next, this->state); int num_waypoints_to_remove = std::ceil(this->state->config.pathing.upload_distance_buffer_m / @@ -57,9 +54,9 @@ void PathGenTick::startPathGeneration() { coverage = MissionPath(MissionPath::Type::HOVER, coverage_gps, this->state->config.pathing.coverage.hover.hover_time_s); } - MissionPath waypoint = MissionPath(MissionPath::Type::FORWARD, init_gps); - this->state->setInitPath(waypoint); - + + this->state->setInitPath(init); + this->state->setNextWaypointPath(next); this->state->setCoveragePath(coverage); }); } From 7e6634d66d76b66f1045308472dabdf94def6ca9 Mon Sep 17 00:00:00 2001 From: AskewParity Date: Tue, 12 May 2026 16:03:11 -0700 Subject: [PATCH 06/17] fix: prevent duplicate path decrement --- src/ticks/fly_waypoints.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/ticks/fly_waypoints.cpp b/src/ticks/fly_waypoints.cpp index 34bcabbf..90368200 100644 --- a/src/ticks/fly_waypoints.cpp +++ b/src/ticks/fly_waypoints.cpp @@ -80,10 +80,7 @@ Tick* FlyWaypointsTick::tick() { return nullptr; } - if (state->getLapsRemaining() > 1) { - state->decrementLapsRemaining(); - return new MavUploadTick( this->state, new FlyWaypointsTick(this->state, new FlySearchTick(this->state)), state->getNextWaypointPath(), false); From e1d523917337021c5104f42947138014221045e5 Mon Sep 17 00:00:00 2001 From: AskewParity Date: Tue, 12 May 2026 16:06:51 -0700 Subject: [PATCH 07/17] feat: add clear mav mission --- include/network/mavlink.hpp | 6 +++--- src/network/gcs_routes.cpp | 2 +- src/network/mavlink.cpp | 17 ++++++++++++++++- 3 files changed, 20 insertions(+), 5 deletions(-) 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/src/network/gcs_routes.cpp b/src/network/gcs_routes.cpp index 0247f588..b735bf39 100644 --- a/src/network/gcs_routes.cpp +++ b/src/network/gcs_routes.cpp @@ -573,7 +573,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 8d19e065..3dba2099 100644 --- a/src/network/mavlink.cpp +++ b/src/network/mavlink.cpp @@ -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(); } From d2eb3e63adffb6ce6a7affa3381ae5472b4735ed Mon Sep 17 00:00:00 2001 From: AskewParity Date: Tue, 12 May 2026 16:18:58 -0700 Subject: [PATCH 08/17] fix: change mavlink mission upload scheme --- src/ticks/fly_search.cpp | 17 +---------------- src/ticks/fly_waypoints.cpp | 17 ++--------------- src/ticks/mav_upload.cpp | 1 + 3 files changed, 4 insertions(+), 31 deletions(-) diff --git a/src/ticks/fly_search.cpp b/src/ticks/fly_search.cpp index 685f0bff..f97199ad 100644 --- a/src/ticks/fly_search.cpp +++ b/src/ticks/fly_search.cpp @@ -23,32 +23,17 @@ std::chrono::milliseconds FlySearchTick::getWait() const { } void FlySearchTick::init() { + // TODO: can we delete the following camera lines? this->state->getCamera()->startStreaming(); 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"); - } - LOG_F(INFO, "Total Waypoint #: %zu", this->state->getMav()->totalWaypoints()); } Tick* FlySearchTick::tick() { - if (!this->mission_started) { - this->mission_started = this->state->getMav()->startMission(); - return nullptr; - } - bool isMissionFinished = state->getMav()->isMissionFinished(); if (isMissionFinished) { diff --git a/src/ticks/fly_waypoints.cpp b/src/ticks/fly_waypoints.cpp index 90368200..bd2f1b25 100644 --- a/src/ticks/fly_waypoints.cpp +++ b/src/ticks/fly_waypoints.cpp @@ -18,22 +18,9 @@ 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"); - } - 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()); } @@ -80,7 +67,7 @@ Tick* FlyWaypointsTick::tick() { return nullptr; } - if (state->getLapsRemaining() > 1) { + if (state->getLapsRemaining() > 0) { return new MavUploadTick( this->state, new FlyWaypointsTick(this->state, new FlySearchTick(this->state)), state->getNextWaypointPath(), false); diff --git a/src/ticks/mav_upload.cpp b/src/ticks/mav_upload.cpp index 24e18518..225c7270 100644 --- a/src/ticks/mav_upload.cpp +++ b/src/ticks/mav_upload.cpp @@ -24,6 +24,7 @@ std::chrono::milliseconds MavUploadTick::getWait() const { } void MavUploadTick::init() { + this->state->getMav()->clearMission(); this->mav_uploaded = std::async(std::launch::async, &MavlinkClient::uploadMissionUntilSuccess, this->state->getMav(), From a6f8cd6f607da51ebcd3f78c77603946b353c47c Mon Sep 17 00:00:00 2001 From: AskewParity Date: Tue, 12 May 2026 19:42:41 -0700 Subject: [PATCH 09/17] feat: unit tests + refactor code --- include/pathing/static.hpp | 12 ++- src/network/gcs_routes.cpp | 1 + src/pathing/static.cpp | 27 ++--- src/ticks/path_gen.cpp | 4 +- tests/unit/pathing_test.cpp | 208 ++++++++++++++++++++++++++++-------- 5 files changed, 193 insertions(+), 59 deletions(-) diff --git a/include/pathing/static.hpp b/include/pathing/static.hpp index d1c2fc97..c73b1e90 100644 --- a/include/pathing/static.hpp +++ b/include/pathing/static.hpp @@ -302,6 +302,16 @@ class AirdropApproachPathing { */ RRTPoint getCurrentLoc(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 @@ -310,8 +320,6 @@ generateNextWaypointPath(std::shared_ptr state, double start_angle std::vector generateSearchPath(std::shared_ptr state, double start_angle); -double calculateFinalAngle(const MissionPath& path, std::shared_ptr state); - std::vector generateAirdropApproach(std::shared_ptr state, const GPSCoord &goal); diff --git a/src/network/gcs_routes.cpp b/src/network/gcs_routes.cpp index b735bf39..bd02f79e 100644 --- a/src/network/gcs_routes.cpp +++ b/src/network/gcs_routes.cpp @@ -122,6 +122,7 @@ DEF_GCS_HANDLE(Post, mission) { state->setCartesianConverter(CartesianConverter(mission.flightboundary())); auto err = state->mission_params.setMission(mission, state->getCartesianConverter().value()); + std::cout << "Hello" << std::endl; if (err.has_value()) { LOG_RESPONSE(WARNING, err.value().c_str(), BAD_REQUEST); } else { diff --git a/src/pathing/static.cpp b/src/pathing/static.cpp index cea4a0c7..2799d55c 100644 --- a/src/pathing/static.cpp +++ b/src/pathing/static.cpp @@ -619,6 +619,21 @@ RRTPoint getCurrentLoc(std::shared_ptr state) { return RRTPoint(start_xyz, start_angle); } +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 @@ -696,18 +711,6 @@ generateNextWaypointPath(std::shared_ptr state, double start_angle return output_coords; } -double calculateFinalAngle(const MissionPath& path, std::shared_ptr state) { - const auto& coords = path.get(); - if (coords.size() < 2) { - return 0.0; - } - - XYZCoord pt1 = state->getCartesianConverter()->toXYZ(coords[coords.size() - 2]); - XYZCoord pt2 = state->getCartesianConverter()->toXYZ(coords[coords.size() - 1]); - - return std::atan2(pt2.y - pt1.y, pt2.x - pt1.x); -} - std::vector generateSearchPath(std::shared_ptr state, double start_angle) { std::vector gps_coords; if (state->config.pathing.coverage.method == AirdropCoverageMethod::Enum::FORWARD) { diff --git a/src/ticks/path_gen.cpp b/src/ticks/path_gen.cpp index 6e900717..bd3bd3d4 100644 --- a/src/ticks/path_gen.cpp +++ b/src/ticks/path_gen.cpp @@ -35,11 +35,11 @@ void PathGenTick::startPathGeneration() { this->paths_future = std::async(std::launch::async, [this]() { std::vector init_gps = generateInitialPath(this->state); MissionPath init = MissionPath(MissionPath::Type::FORWARD, init_gps); - double angle1 = calculateFinalAngle(init, this->state); + double angle1 = calculateFinalAngle(init, this->state->getCartesianConverter()); std::vector next_gps = generateNextWaypointPath(this->state, angle1); MissionPath next = MissionPath(MissionPath::Type::FORWARD, next_gps); - double angle2 = calculateFinalAngle(next, this->state); + double angle2 = calculateFinalAngle(next, this->state->getCartesianConverter()); int num_waypoints_to_remove = std::ceil(this->state->config.pathing.upload_distance_buffer_m / 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 From 6deacc427bd3112a5dd2c8ab16ce5729d7ae5217 Mon Sep 17 00:00:00 2001 From: AskewParity Date: Tue, 12 May 2026 19:43:58 -0700 Subject: [PATCH 10/17] chore: lint --- src/pathing/static.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/pathing/static.cpp b/src/pathing/static.cpp index 2799d55c..c56d8ca7 100644 --- a/src/pathing/static.cpp +++ b/src/pathing/static.cpp @@ -619,13 +619,14 @@ RRTPoint getCurrentLoc(std::shared_ptr state) { return RRTPoint(start_xyz, start_angle); } -double calculateFinalAngle(const MissionPath& path, - const std::optional> cartesianConverter) { +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 + 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]); From d1d8190b202b1f9a4f5897d5ca67cca5e6d89bf9 Mon Sep 17 00:00:00 2001 From: AskewParity Date: Tue, 12 May 2026 19:51:19 -0700 Subject: [PATCH 11/17] chore: revert cout in gcs_routes --- src/network/gcs_routes.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/network/gcs_routes.cpp b/src/network/gcs_routes.cpp index bd02f79e..b735bf39 100644 --- a/src/network/gcs_routes.cpp +++ b/src/network/gcs_routes.cpp @@ -122,7 +122,6 @@ DEF_GCS_HANDLE(Post, mission) { state->setCartesianConverter(CartesianConverter(mission.flightboundary())); auto err = state->mission_params.setMission(mission, state->getCartesianConverter().value()); - std::cout << "Hello" << std::endl; if (err.has_value()) { LOG_RESPONSE(WARNING, err.value().c_str(), BAD_REQUEST); } else { From 0bb8a9e4064b939f424ba81b0a792ecb4b5a5649 Mon Sep 17 00:00:00 2001 From: AskewParity Date: Thu, 14 May 2026 17:23:35 -0700 Subject: [PATCH 12/17] chore: ensure mission starts --- src/ticks/fly_search.cpp | 8 ++++++-- src/ticks/fly_waypoints.cpp | 6 +++++- 2 files changed, 11 insertions(+), 3 deletions(-) diff --git a/src/ticks/fly_search.cpp b/src/ticks/fly_search.cpp index f97199ad..44dcd350 100644 --- a/src/ticks/fly_search.cpp +++ b/src/ticks/fly_search.cpp @@ -2,6 +2,7 @@ #include #include +#include #include "ticks/ids.hpp" #include "utilities/common.hpp" @@ -23,17 +24,20 @@ std::chrono::milliseconds FlySearchTick::getWait() const { } void FlySearchTick::init() { - // TODO: can we delete the following camera lines? this->state->getCamera()->startStreaming(); this->airdrop_boundary = this->state->mission_params.getAirdropBoundary(); this->last_photo_time = getUnixTime_ms(); - this->mission_started = this->state->getMav()->startMission(); + 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()); } Tick* FlySearchTick::tick() { + bool isMissionFinished = state->getMav()->isMissionFinished(); if (isMissionFinished) { diff --git a/src/ticks/fly_waypoints.cpp b/src/ticks/fly_waypoints.cpp index bd2f1b25..3384ff41 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,7 +19,10 @@ FlyWaypointsTick::FlyWaypointsTick(std::shared_ptr state, Tick* ne : Tick(state, TickID::FlyWaypoints), next_tick(next_tick) {} void FlyWaypointsTick::init() { - this->mission_started = this->state->getMav()->startMission(); + while (!this->mission_started) { + this->mission_started = this->state->getMav()->startMission(); + std::this_thread::sleep_for(100ms); + } state->decrementLapsRemaining(); LOG_F(INFO, "Started FlyWaypointsTick, Laps Remaining: %d", state->getLapsRemaining()); From 211120335356eb5425f3a725f1b4643489a2374a Mon Sep 17 00:00:00 2001 From: AskewParity Date: Thu, 14 May 2026 17:24:14 -0700 Subject: [PATCH 13/17] chore: ensure that path has enough points before deleting --- src/ticks/path_gen.cpp | 15 ++++++++++----- 1 file changed, 10 insertions(+), 5 deletions(-) diff --git a/src/ticks/path_gen.cpp b/src/ticks/path_gen.cpp index bd3bd3d4..8da65e94 100644 --- a/src/ticks/path_gen.cpp +++ b/src/ticks/path_gen.cpp @@ -33,19 +33,24 @@ Tick* PathGenTick::tick() { void PathGenTick::startPathGeneration() { 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()); - int num_waypoints_to_remove = - std::ceil(this->state->config.pathing.upload_distance_buffer_m / - this->state->config.pathing.dubins.point_separation); - std::vector coverage_gps = generateSearchPath(this->state, angle2); - coverage_gps.erase(coverage_gps.begin(), coverage_gps.begin() + num_waypoints_to_remove);; + + 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) { From 25e5f6be6879a18dad69eec27e88169ed4d84e39 Mon Sep 17 00:00:00 2001 From: AskewParity Date: Thu, 14 May 2026 17:49:03 -0700 Subject: [PATCH 14/17] chore: lint --- src/ticks/fly_search.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/ticks/fly_search.cpp b/src/ticks/fly_search.cpp index 44dcd350..ddc10f4c 100644 --- a/src/ticks/fly_search.cpp +++ b/src/ticks/fly_search.cpp @@ -37,7 +37,6 @@ void FlySearchTick::init() { } Tick* FlySearchTick::tick() { - bool isMissionFinished = state->getMav()->isMissionFinished(); if (isMissionFinished) { From d98f28acd96880677a0c91024f73ceadfa89e00e Mon Sep 17 00:00:00 2001 From: AskewParity Date: Thu, 14 May 2026 17:56:31 -0700 Subject: [PATCH 15/17] feat: assert existance of cartesean converter --- src/ticks/path_gen.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/ticks/path_gen.cpp b/src/ticks/path_gen.cpp index 8da65e94..eb792507 100644 --- a/src/ticks/path_gen.cpp +++ b/src/ticks/path_gen.cpp @@ -18,6 +18,7 @@ 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(); } From 6604c87d17fb72c07ec95d8ea4ef9d10cead432f Mon Sep 17 00:00:00 2001 From: AskewParity Date: Thu, 14 May 2026 17:57:28 -0700 Subject: [PATCH 16/17] chore: pass cartesean converter by reference --- include/pathing/static.hpp | 5 +++-- src/pathing/static.cpp | 2 +- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/include/pathing/static.hpp b/include/pathing/static.hpp index c73b1e90..b8a615b9 100644 --- a/include/pathing/static.hpp +++ b/include/pathing/static.hpp @@ -309,8 +309,9 @@ RRTPoint getCurrentLoc(std::shared_ptr state); * @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); +double calculateFinalAngle( + const MissionPath& path, + const std::optional>& cartesianConverter); std::vector generateInitialPath(std::shared_ptr state); diff --git a/src/pathing/static.cpp b/src/pathing/static.cpp index c56d8ca7..200ae2ea 100644 --- a/src/pathing/static.cpp +++ b/src/pathing/static.cpp @@ -621,7 +621,7 @@ RRTPoint getCurrentLoc(std::shared_ptr state) { double calculateFinalAngle( const MissionPath& path, - const std::optional> cartesianConverter) { + const std::optional>& cartesianConverter) { const auto& coords = path.get(); if (coords.size() < 2) { From b873ba9997d658ed98452bcff66ff071570477a8 Mon Sep 17 00:00:00 2001 From: AskewParity Date: Tue, 19 May 2026 14:13:49 -0700 Subject: [PATCH 17/17] chore: create check for search path generation --- src/pathing/static.cpp | 129 +++++++++++++++++++++-------------------- 1 file changed, 65 insertions(+), 64 deletions(-) diff --git a/src/pathing/static.cpp b/src/pathing/static.cpp index 200ae2ea..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); } @@ -622,11 +617,10 @@ RRTPoint getCurrentLoc(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 + 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]); @@ -665,15 +659,15 @@ std::vector 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 output_coords; } -std::vector -generateNextWaypointPath(std::shared_ptr state, double start_angle) { +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", @@ -705,7 +699,7 @@ generateNextWaypointPath(std::shared_ptr state, double start_angle 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)); } @@ -715,14 +709,21 @@ generateNextWaypointPath(std::shared_ptr state, double start_angle std::vector generateSearchPath(std::shared_ptr state, double start_angle) { std::vector gps_coords; if (state->config.pathing.coverage.method == AirdropCoverageMethod::Enum::FORWARD) { + 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)); } @@ -730,15 +731,15 @@ std::vector generateSearchPath(std::shared_ptr state, do } 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 gps_coords; } } -std::vector -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 @@ -765,7 +766,7 @@ generateAirdropApproach(std::shared_ptr state, const GPSCoord &goa 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)); }