diff --git a/configs/dev.json b/configs/dev.json index e23bab3c..20407576 100644 --- a/configs/dev.json +++ b/configs/dev.json @@ -13,7 +13,8 @@ } }, "takeoff": { - "altitude_m": 30.0 + "altitude_m": 30.0, + "payload_size": 2 }, "pathing": { "dubins": { @@ -25,7 +26,8 @@ "rewire_radius": 256.0, "optimize": true, "point_fetch_method": "nearest", - "allowed_to_skip_waypoints": false + "allowed_to_skip_waypoints": false, + "generate_deviations": false }, "coverage": { "altitude_m": 30.0, diff --git a/include/core/mission_parameters.hpp b/include/core/mission_parameters.hpp index b44fd70b..f8d61a36 100644 --- a/include/core/mission_parameters.hpp +++ b/include/core/mission_parameters.hpp @@ -30,6 +30,7 @@ class MissionParameters { Polygon getFlightBoundary(); Polygon getAirdropBoundary(); + Polygon getMappingBoundary(); Polyline getWaypoints(); // CHANGE: Return type is now std::vector std::vector getAirdrops(); @@ -39,7 +40,7 @@ class MissionParameters { // Important to use this instead of the singular getters // to avoid race conditions // CHANGE: Tuple element type is now std::vector - std::tuple> getConfig(); + std::tuple> getConfig(); // returns error string to be displayed back to the user // Ensure CartesianConverter template parameter matches usage @@ -54,6 +55,7 @@ class MissionParameters { Polygon flightBoundary; Polygon airdropBoundary; + Polygon mappingBoundary; Polyline waypoints; // CHANGE: Member variable type is now std::vector std::vector airdrops; diff --git a/include/ticks/ids.hpp b/include/ticks/ids.hpp index 735c9c6e..7bd6a865 100644 --- a/include/ticks/ids.hpp +++ b/include/ticks/ids.hpp @@ -19,6 +19,7 @@ enum class TickID { MissionDone, ActiveTakeoff, WaitForTakeoff, + Refueling, }; #define _SET_TICK_ID_MAPPING(id) \ @@ -41,6 +42,7 @@ constexpr const char* TICK_ID_TO_STR(TickID id) { _SET_TICK_ID_MAPPING(MissionDone); _SET_TICK_ID_MAPPING(ActiveTakeoff); _SET_TICK_ID_MAPPING(WaitForTakeoff); + _SET_TICK_ID_MAPPING(Refueling); default: return "Unknown TickID"; } } diff --git a/include/ticks/manual_landing.hpp b/include/ticks/manual_landing.hpp index fdfb6c08..82f49a78 100644 --- a/include/ticks/manual_landing.hpp +++ b/include/ticks/manual_landing.hpp @@ -11,11 +11,14 @@ */ class ManualLandingTick : public Tick { public: - explicit ManualLandingTick(std::shared_ptr state); + explicit ManualLandingTick(std::shared_ptr state, Tick* next_tick); std::chrono::milliseconds getWait() const override; Tick* tick() override; + + private: + Tick* next_tick; }; #endif // INCLUDE_TICKS_MANUAL_LANDING_HPP_ diff --git a/include/ticks/refueling.hpp b/include/ticks/refueling.hpp new file mode 100644 index 00000000..adef851c --- /dev/null +++ b/include/ticks/refueling.hpp @@ -0,0 +1,21 @@ +#ifndef INCLUDE_TICKS_REFUELING_HPP_ +#define INCLUDE_TICKS_REFUELING_HPP_ + +#include +#include + +#include "ticks/tick.hpp" + +/* + * Wait for the refueling process to finish. + */ +class RefuelingTick : public Tick { + public: + explicit RefuelingTick(std::shared_ptr state); + + std::chrono::milliseconds getWait() const override; + + Tick* tick() override; +}; + +#endif // INCLUDE_TICKS_REFUELING_HPP_ diff --git a/include/utilities/constants.hpp b/include/utilities/constants.hpp index 240e9bba..732a2a2b 100644 --- a/include/utilities/constants.hpp +++ b/include/utilities/constants.hpp @@ -77,6 +77,7 @@ const std::chrono::milliseconds AUTO_LANDING_TICK_WAIT = std::chrono::millisecon const std::chrono::milliseconds MISSION_DONE_TICK_WAIT = std::chrono::milliseconds(100); const std::chrono::milliseconds ACTIVE_TAKEOFF_TICK_WAIT = std::chrono::milliseconds(100); const std::chrono::milliseconds WAIT_FOR_TAKEOFF_TICK_WAIT = std::chrono::milliseconds(100); +const std::chrono::milliseconds REFUELING_TICK_WAIT = std::chrono::milliseconds(100); const double EARTH_RADIUS_METERS = 6378137.0; diff --git a/include/utilities/obc_config.hpp b/include/utilities/obc_config.hpp index 97532bef..0bc50d37 100644 --- a/include/utilities/obc_config.hpp +++ b/include/utilities/obc_config.hpp @@ -32,6 +32,7 @@ struct NetworkConfig { struct TakeoffConfig { float altitude_m; + int payload_size; }; struct CVConfig { @@ -63,6 +64,7 @@ struct RRTConfig { PointFetchMethod::Enum point_fetch_method; bool allowed_to_skip_waypoints; // if true, will skip waypoints if it can not connect after 1 // RRT iteration + bool generate_deviations; }; namespace AirdropCoverageMethod { diff --git a/protos b/protos index ab5865e5..df972e41 160000 --- a/protos +++ b/protos @@ -1 +1 @@ -Subproject commit ab5865e5b5f65c561530e6741b22b1b2b30e3f85 +Subproject commit df972e412ca1e93b2d799bc906d7a906e9b62505 diff --git a/src/core/mission_parameters.cpp b/src/core/mission_parameters.cpp index 709e19df..ac057fef 100644 --- a/src/core/mission_parameters.cpp +++ b/src/core/mission_parameters.cpp @@ -49,6 +49,11 @@ Polygon MissionParameters::getAirdropBoundary() { return this->airdropBoundary; } +Polygon MissionParameters::getMappingBoundary() { + ReadLock lock(this->mut); + return this->mappingBoundary; +} + Polyline MissionParameters::getWaypoints() { ReadLock lock(this->mut); return this->waypoints; @@ -59,10 +64,12 @@ std::vector MissionParameters::getAirdrops() { return this->airdrops; } -std::tuple> MissionParameters::getConfig() { +std::tuple> +MissionParameters::getConfig() { ReadLock lock(this->mut); - return std::make_tuple(this->flightBoundary, this->airdropBoundary, this->waypoints, + return std::make_tuple(this->flightBoundary, this->airdropBoundary, + this->mappingBoundary, this->waypoints, this->airdrops); } @@ -94,6 +101,11 @@ std::optional MissionParameters::setMission( if (mission.airdropboundary().size() < 3) { err += "Airdrop boundary must have at least 3 coordinates."; } + + if (mission.mappingboundary().size() < 3) { + err += "Mapping boundary must have at least 3 coordinates."; + } + if (!err.empty()) { return err; } @@ -101,6 +113,7 @@ std::optional MissionParameters::setMission( this->cached_mission = mission; this->flightBoundary = cconverter.toXYZ(mission.flightboundary()); this->airdropBoundary = cconverter.toXYZ(mission.airdropboundary()); + this->mappingBoundary = cconverter.toXYZ(mission.mappingboundary()); this->waypoints = cconverter.toXYZ(mission.waypoints()); for (const auto& airdrop : mission.airdropassignments()) { // Use const& for efficiency this->_setAirdrop(airdrop); diff --git a/src/cv/pipeline.cpp b/src/cv/pipeline.cpp index d53a724c..adac69e2 100644 --- a/src/cv/pipeline.cpp +++ b/src/cv/pipeline.cpp @@ -74,9 +74,6 @@ PipelineResults Pipeline::run(const ImageData& imageData) { GPSCoord targetPosition; if (imageData.TELEMETRY.has_value()) { targetPosition = this->gsdLocalizer.localize(imageData.TELEMETRY.value(), box); - std::cout << "pipeline: " << - targetPosition.latitude() << " " - << targetPosition.longitude(); } diff --git a/src/pathing/static.cpp b/src/pathing/static.cpp index e9abbe5a..c56603f9 100644 --- a/src/pathing/static.cpp +++ b/src/pathing/static.cpp @@ -451,10 +451,11 @@ std::vector HoverCoveragePathing::run() { this->drop_zone.size()); } - XYZCoord top_left = this->drop_zone.at(3); - XYZCoord top_right = this->drop_zone.at(2); + // Input Coordinates MUST BE in this order XYZCoord bottom_left = this->drop_zone.at(0); XYZCoord bottom_right = this->drop_zone.at(1); + XYZCoord top_right = this->drop_zone.at(2); + XYZCoord top_left = this->drop_zone.at(3); std::vector hover_points; @@ -545,11 +546,11 @@ std::vector> generateGoalListDeviations(const std::vector< } std::vector> generateRankedNewGoalsList(const std::vector &goals, - const Environment &airspace) { + const Environment &mapping_bounds) { // generate deviation points randomly in the mapping region std::vector deviation_points; for (int i = 0; i < 200; i++) { - deviation_points.push_back(airspace.getRandomPoint(true)); + deviation_points.push_back(mapping_bounds.getRandomPoint(true)); printf("Deviation point: %f, %f\n", deviation_points.back().x, deviation_points.back().y); } @@ -565,7 +566,7 @@ std::vector> generateRankedNewGoalsList(const std::vector< // run each goal list and get the area covered and the length of the path std::vector> area_length_pairs; for (const std::vector &new_goals : new_goals_list) { - area_length_pairs.push_back(airspace.estimateAreaCoveredAndPathLength(new_goals)); + area_length_pairs.push_back(mapping_bounds.estimateAreaCoveredAndPathLength(new_goals)); } // rank the new goal lists by the area covered and the length of the path @@ -607,9 +608,10 @@ MissionPath generateInitialPath(std::shared_ptr state) { } // update goals here - // create a dummy environment - Environment airspace(state->mission_params.getFlightBoundary(), {}, {}, goals, {}); - goals = generateRankedNewGoalsList(goals, airspace)[0]; + if (state->config.pathing.rrt.generate_deviations) { + Environment mapping_bounds(state->mission_params.getMappingBoundary(), {}, {}, goals, {}); + goals = generateRankedNewGoalsList(goals, mapping_bounds)[0]; + } double init_angle = std::atan2(goals.front().y - state->mission_params.getWaypoints().front().y, @@ -633,17 +635,29 @@ MissionPath generateInitialPath(std::shared_ptr state) { } MissionPath generateSearchPath(std::shared_ptr state) { + std::vector gps_coords; if (state->config.pathing.coverage.method == AirdropCoverageMethod::Enum::FORWARD) { LOG_F(FATAL, "Forward search path not fully integrated yet."); + + RRTPoint start(state->mission_params.getWaypoints().front(), 0); + + // TODO , change the starting point to be something closer to loiter + // region + ForwardCoveragePathing pathing(start, SEARCH_RADIUS, + state->mission_params.getFlightBoundary(), + state->mission_params.getAirdropBoundary(), state->config); + + for (const auto &coord : pathing.run()) { + gps_coords.push_back(state->getCartesianConverter()->toLatLng(coord)); + } + return MissionPath(MissionPath::Type::FORWARD, {}); } else { // hover HoverCoveragePathing pathing(state); - std::vector gps_coords; for (const auto &coord : pathing.run()) { gps_coords.push_back(state->getCartesianConverter()->toLatLng(coord)); } - return MissionPath(MissionPath::Type::HOVER, gps_coords, state->config.pathing.coverage.hover.hover_time_s); } diff --git a/src/ticks/CMakeLists.txt b/src/ticks/CMakeLists.txt index f2e987dd..616c5804 100644 --- a/src/ticks/CMakeLists.txt +++ b/src/ticks/CMakeLists.txt @@ -16,6 +16,7 @@ set(FILES takeoff.cpp active_takeoff.cpp wait_for_takeoff.cpp + refueling.cpp ) set(LIB_DEPS diff --git a/src/ticks/active_takeoff.cpp b/src/ticks/active_takeoff.cpp index e1007533..4b7b93ab 100644 --- a/src/ticks/active_takeoff.cpp +++ b/src/ticks/active_takeoff.cpp @@ -5,6 +5,7 @@ #include "ticks/ids.hpp" #include "utilities/constants.hpp" +#include "ticks/airdrop_prep.hpp" #include "ticks/fly_waypoints.hpp" #include "ticks/mav_upload.hpp" #include "ticks/mission_prep.hpp" @@ -50,7 +51,13 @@ Tick* ActiveTakeoffTick::tick() { // NOTE: keep in sync with manual takeoff tick // transitions to flying waypoints tick, such that when the flying waypoints // tick is done it transitions to uploading the coverage path - return new FlyWaypointsTick(this->state, new MavUploadTick( - this->state, new FlySearchTick(this->state), - state->getCoveragePath(), false)); + + if (state->getDroppedAirdrops().size() >= this->state->config.takeoff.payload_size) { + return new FlyWaypointsTick(this->state, new AirdropPrepTick(this->state)); + // return new AirdropPrepTick(this->state); + } else { + return new FlyWaypointsTick(this->state, new MavUploadTick( + this->state, new FlySearchTick(this->state), + state->getCoveragePath(), false)); + } } diff --git a/src/ticks/airdrop_approach.cpp b/src/ticks/airdrop_approach.cpp index 97fb2732..ddb297be 100644 --- a/src/ticks/airdrop_approach.cpp +++ b/src/ticks/airdrop_approach.cpp @@ -8,6 +8,8 @@ #include "ticks/airdrop_prep.hpp" #include "ticks/manual_landing.hpp" #include "utilities/constants.hpp" +#include "ticks/refueling.hpp" +#include "ticks/wait_for_takeoff.hpp" AirdropApproachTick::AirdropApproachTick(std::shared_ptr state) :Tick(state, TickID::AirdropApproach) {} @@ -35,7 +37,9 @@ Tick* AirdropApproachTick::tick() { if (state->getMav()->isMissionFinished()) { if (state->getDroppedAirdrops().size() >= NUM_AIRDROPS) { - return new ManualLandingTick(state); + return new ManualLandingTick(state, nullptr); + } else if (state->getDroppedAirdrops().size() % state->config.takeoff.payload_size == 0) { + return new ManualLandingTick(state, new RefuelingTick(state)); } else { return new MavUploadTick(state, new FlyWaypointsTick(state, new AirdropPrepTick(state)), state->getInitPath(), false); diff --git a/src/ticks/airdrop_prep.cpp b/src/ticks/airdrop_prep.cpp index b2e158c4..c22191d6 100644 --- a/src/ticks/airdrop_prep.cpp +++ b/src/ticks/airdrop_prep.cpp @@ -23,7 +23,7 @@ Tick* AirdropPrepTick::tick() { auto dropped_airdrops = state->getDroppedAirdrops(); if (dropped_airdrops.size() >= NUM_AIRDROPS) { - return new ManualLandingTick(state); + return new ManualLandingTick(state, nullptr); } LockPtr results = state->getCV()->getMatchedResults(); diff --git a/src/ticks/manual_landing.cpp b/src/ticks/manual_landing.cpp index a40be2df..cf5f56bb 100644 --- a/src/ticks/manual_landing.cpp +++ b/src/ticks/manual_landing.cpp @@ -5,13 +5,17 @@ #include "ticks/ids.hpp" #include "utilities/constants.hpp" -ManualLandingTick::ManualLandingTick(std::shared_ptr state) - :Tick(state, TickID::ManualLanding) {} +ManualLandingTick::ManualLandingTick(std::shared_ptr state, Tick* next_tick) + :Tick(state, TickID::ManualLanding), next_tick(next_tick) {} std::chrono::milliseconds ManualLandingTick::getWait() const { return MANUAL_LANDING_TICK_WAIT; } Tick* ManualLandingTick::tick() { + // TODO: Test this in mock testflight + if (!state.get()->getMav()->isArmed()) { + return next_tick; + } return nullptr; } diff --git a/src/ticks/refueling.cpp b/src/ticks/refueling.cpp new file mode 100644 index 00000000..65f036bd --- /dev/null +++ b/src/ticks/refueling.cpp @@ -0,0 +1,22 @@ +#include "ticks/refueling.hpp" + +#include + +#include "ticks/active_takeoff.hpp" +#include "ticks/ids.hpp" +#include "ticks/mav_upload.hpp" +#include "utilities/constants.hpp" + +RefuelingTick::RefuelingTick(std::shared_ptr state) + : Tick(state, TickID::Refueling) {} + +std::chrono::milliseconds RefuelingTick::getWait() const { return REFUELING_TICK_WAIT; } + +Tick* RefuelingTick::tick() { + if (state.get()->getMav()->isArmed()) { + return new MavUploadTick(state, new WaitForTakeoffTick(state), state->getInitPath(), false); + } + + return nullptr; + // return new MavUploadTick(state, new WaitForTakeoffTick(state), state->getInitPath(), false); +} diff --git a/src/ticks/takeoff.cpp b/src/ticks/takeoff.cpp index 3dc126ae..4cfe0560 100644 --- a/src/ticks/takeoff.cpp +++ b/src/ticks/takeoff.cpp @@ -22,6 +22,8 @@ Tick* TakeoffTick::tick() { // NOTE: keep in sync with active_takeoff tick // transitions to flying waypoints tick, such that when the flying waypoints // tick is done it transitions to uploading the coverage path + + // This is MANUAL Takeoff (idk why its named so confusing) return new FlyWaypointsTick(this->state, new MavUploadTick( this->state, new FlySearchTick(this->state), state->getCoveragePath(), false)); diff --git a/src/utilities/obc_config.cpp b/src/utilities/obc_config.cpp index 70a92661..ef08bec5 100644 --- a/src/utilities/obc_config.cpp +++ b/src/utilities/obc_config.cpp @@ -61,6 +61,7 @@ OBCConfig::OBCConfig(int argc, char* argv[]) { SET_CONFIG_OPT(pathing, rrt, iterations_per_waypoint); SET_CONFIG_OPT(pathing, rrt, rewire_radius); SET_CONFIG_OPT(pathing, rrt, optimize); + SET_CONFIG_OPT(pathing, rrt, generate_deviations); SET_CONFIG_OPT_VARIANT(PointFetchMethod, pathing, rrt, point_fetch_method); @@ -95,6 +96,7 @@ OBCConfig::OBCConfig(int argc, char* argv[]) { SET_CONFIG_OPT(camera, mock, images_dir); SET_CONFIG_OPT(takeoff, altitude_m); + SET_CONFIG_OPT(takeoff, payload_size); std::string common_params_path = params_dir / "common.json"; std::ifstream common_params_stream(common_params_path);