diff --git a/configs/config.json b/configs/config.json index 9c40cd03..d87367aa 100644 --- a/configs/config.json +++ b/configs/config.json @@ -23,6 +23,7 @@ }, "coverage": { "altitude_m": 30.0, + "camera_vision_m": 20, "method": "hover", "hover": { "pictures_per_stop": 1, diff --git a/configs/dev-config.json b/configs/dev-config.json index b8b45c04..284400db 100644 --- a/configs/dev-config.json +++ b/configs/dev-config.json @@ -23,6 +23,7 @@ }, "coverage": { "altitude_m": 30.0, + "camera_vision_m": 20, "method": "hover", "hover": { "pictures_per_stop": 1, diff --git a/include/pathing/static.hpp b/include/pathing/static.hpp index 6962a85b..9d14160a 100644 --- a/include/pathing/static.hpp +++ b/include/pathing/static.hpp @@ -197,14 +197,17 @@ class RRT { */ class ForwardCoveragePathing { public: - ForwardCoveragePathing(const RRTPoint &start, double scan_radius, Polygon bounds, Polygon airdrop_zone, - std::vector obstacles = {}, - AirdropCoverageConfig config = {.altitude_m = 30.0, - .method = AirdropCoverageMethod::Enum::FORWARD, - .hover = {}, - .forward = {.optimize = false, - .vertical = false, - .one_way = false}}); + ForwardCoveragePathing(const RRTPoint &start, + double scan_radius, + Polygon bounds, + Polygon airdrop_zone, + std::vector obstacles = {}, + AirdropCoverageConfig config = {.altitude_m = 30.0, + .method = AirdropCoverageMethod::Enum::FORWARD, + .hover = {}, + .forward = {.optimize = false, + .vertical = false, + .one_way = false}}); /** * Generates a path of parallel lines to cover a given area diff --git a/include/utilities/obc_config.hpp b/include/utilities/obc_config.hpp index 90c7ae7e..04870691 100644 --- a/include/utilities/obc_config.hpp +++ b/include/utilities/obc_config.hpp @@ -3,12 +3,15 @@ #include #include +#include +#include #include #include "udp_squared/internal/enum.h" #include "utilities/constants.hpp" #include "utilities/datatypes.hpp" -#define CONFIG_VARIANT_MAPPING_T(enum_type) const std::initializer_list> +#define CONFIG_VARIANT_MAPPING_T(enum_type) \ + const std::initializer_list> struct LoggingConfig { std::string dir; @@ -43,7 +46,7 @@ namespace PointFetchMethod { CONFIG_VARIANT_MAPPING_T(Enum) MAPPINGS = { {"none", Enum::NONE}, {"random", Enum::RANDOM}, {"nearest", Enum::NEAREST} }; -}; +}; // namespace PointFetchMethod struct RRTConfig { int iterations_per_waypoint; // number of iterations run between two waypoints @@ -62,14 +65,15 @@ namespace AirdropCoverageMethod { CONFIG_VARIANT_MAPPING_T(Enum) MAPPINGS = { {"hover", Enum::HOVER}, {"forward", Enum::FORWARD} }; -}; +}; // namespace AirdropCoverageMethod struct AirdropCoverageConfig { double altitude_m; + double camera_vision_m; AirdropCoverageMethod::Enum method; struct { std::size_t pictures_per_stop; - std::time_t hover_time_s; + std::time_t hover_time_s; } hover; struct { bool optimize; // whether to ignore the config below and run all ways. diff --git a/include/utilities/obc_config_macros.hpp b/include/utilities/obc_config_macros.hpp index 61bba4aa..aa8a5bf7 100644 --- a/include/utilities/obc_config_macros.hpp +++ b/include/utilities/obc_config_macros.hpp @@ -1,6 +1,8 @@ #ifndef INCLUDE_UTILITIES_OBC_CONFIG_MACROS_HPP_ #define INCLUDE_UTILITIES_OBC_CONFIG_MACROS_HPP_ +#include + /* ( ) /\ _ ( \ | ( \ ( \.( ) _____ @@ -27,7 +29,8 @@ // just use these macros to do the parsing for the config // it saves like 5 characters, and forces you to use .at instead of [] to index into the json // because .at gives more descriptive error messages -// ALSO: does some shenanegans with tracking the line number of the last call to PARSE and handling errors +// ALSO: does some shenanegans with tracking the line number of the +// last call to PARSE and handling errors // return statement in this macro in catch case is there to supress compiler warning b/c // it doesn't know that FATAL log will kill it @@ -39,8 +42,8 @@ return nlohmann::json(); \ } -// basically setting up an overloaded macro where you can pass in a variable number of args with the same -// macro name. Same setup as in the gcs macros +// basically setting up an overloaded macro where you can pass in a variable number of args +// with the same macro name. Same setup as in the gcs macros #define GET_MACRO_5(_1, _2, _3, _4, _5, NAME, ...) NAME #define PARSE_CONFIG_2(p1, p2) configs.at(#p1).at(#p2) #define PARSE_CONFIG_3(p1, p2, p3) configs.at(#p1).at(#p2).at(#p3) @@ -54,9 +57,8 @@ #define PARSE(...) \ [configs]() { \ HANDLE_JSON_EXCEPTION( \ - GET_MACRO_5(__VA_ARGS__, PARSE_CONFIG_5, PARSE_CONFIG_4, PARSE_CONFIG_3, PARSE_CONFIG_2) \ - (__VA_ARGS__) \ - ) \ + GET_MACRO_5(__VA_ARGS__, PARSE_CONFIG_5, PARSE_CONFIG_4, \ + PARSE_CONFIG_3, PARSE_CONFIG_2) (__VA_ARGS__)) \ }() // look in the code below for an example on how to use this function @@ -70,7 +72,7 @@ constexpr T PARSE_VARIANT(CONFIG_VARIANT_MAPPING_T(T) mapping, std::string input } } LOG_F(FATAL, "Unknown config option %s", input.c_str()); - std::exit(1); // not needed except so the compiler knows this cannot reach end of function + std::exit(1); // not needed except so the compiler knows this cannot reach end of function } // and then at the very end we prevent having to duplicate typing @@ -101,4 +103,4 @@ constexpr T PARSE_VARIANT(CONFIG_VARIANT_MAPPING_T(T) mapping, std::string input GET_MACRO_5(__VA_ARGS__, SET_CONFIG_OPT_5_VARIANT, SET_CONFIG_OPT_4_VARIANT, \ SET_CONFIG_OPT_3_VARIANT, SET_CONFIG_OPT_2_VARIANT) (mapping::MAPPINGS, __VA_ARGS__) -#endif // INCLUDE_UTILITIES_OBC_CONFIG_MACROS_HPP_ \ No newline at end of file +#endif // INCLUDE_UTILITIES_OBC_CONFIG_MACROS_HPP_ diff --git a/src/pathing/static.cpp b/src/pathing/static.cpp index b28a612b..5bff2c8e 100644 --- a/src/pathing/static.cpp +++ b/src/pathing/static.cpp @@ -281,14 +281,13 @@ RRTNode *RRT::parseOptions(const std::vector> &o void RRT::optimizeTree(RRTNode *sample) { tree.RRTStar(sample, rewire_radius); } -ForwardCoveragePathing::ForwardCoveragePathing(const RRTPoint &start, double scan_radius, Polygon bounds, - Polygon airdrop_zone, std::vector obstacles, - AirdropCoverageConfig config) - : start(start), - scan_radius(scan_radius), - airspace(Environment(bounds, airdrop_zone, {}, obstacles)), - dubins(Dubins(TURNING_RADIUS, POINT_SEPARATION)), - config(config) {} +ForwardCoveragePathing::ForwardCoveragePathing( + const RRTPoint &start, double scan_radius, Polygon bounds, + Polygon airdrop_zone, std::vector obstacles, + AirdropCoverageConfig config): + start(start), scan_radius(scan_radius), + airspace(Environment(bounds, airdrop_zone, {}, obstacles)), + dubins(Dubins(TURNING_RADIUS, POINT_SEPARATION)), config(config) {} std::vector ForwardCoveragePathing::run() const { return config.forward.optimize ? coverageOptimal() : coverageDefault(); @@ -362,8 +361,9 @@ std::vector ForwardCoveragePathing::coverageOptimal() const { return generatePath(dubins_paths[best_path_idx], waypoints); } -std::vector ForwardCoveragePathing::generatePath(const std::vector &dubins_options, - const std::vector &waypoints) const { +std::vector ForwardCoveragePathing::generatePath( + const std::vector &dubins_options, + const std::vector &waypoints) const { std::vector path; // height adjustement @@ -398,6 +398,14 @@ std::vector ForwardCoveragePathing::generatePath(const std::vector HoverCoveragePathing::run() { + return {}; +} + AirdropApproachPathing::AirdropApproachPathing(const RRTPoint &start, const XYZCoord &goal, RRTPoint wind, Polygon bounds, std::vector obstacles, @@ -475,12 +483,23 @@ std::vector generateInitialPath(std::shared_ptr state) { int count = 0; for (XYZCoord wpt : path) { - output_coords.push_back(state->getCartesianConverter().value().toLatLng(wpt)); + output_coords.push_back(state->getCartesianConverter()->toLatLng(wpt)); } return output_coords; } std::vector generateSearchPath(std::shared_ptr state) { - return {}; -} \ No newline at end of file + if (state->config.pathing.coverage.method == AirdropCoverageMethod::Enum::FORWARD) { + LOG_F(FATAL, "Forward search path not fully integrated yet."); + return {}; + } else { // hover + const auto& [flight_bounds, airdrop_bounds, _, __] = state->mission_params.getConfig(); + HoverCoveragePathing pathing(flight_bounds, airdrop_bounds); + std::vector coords; + for (const XYZCoord& coord : pathing.run()) { + coords.push_back(state->getCartesianConverter()->toLatLng(coord)); + } + return coords; + } +} diff --git a/src/utilities/obc_config.cpp b/src/utilities/obc_config.cpp index c8ac5687..fff3c2db 100644 --- a/src/utilities/obc_config.cpp +++ b/src/utilities/obc_config.cpp @@ -44,6 +44,7 @@ OBCConfig::OBCConfig(int argc, char* argv[]) { SET_CONFIG_OPT_VARIANT(AirdropCoverageMethod, pathing, coverage, method); SET_CONFIG_OPT(pathing, coverage, altitude_m); + SET_CONFIG_OPT(pathing, coverage, camera_vision_m); SET_CONFIG_OPT(pathing, coverage, hover, hover_time_s); SET_CONFIG_OPT(pathing, coverage, hover, pictures_per_stop); SET_CONFIG_OPT(pathing, coverage, forward, optimize); @@ -81,7 +82,7 @@ OBCConfig::OBCConfig(int argc, char* argv[]) { SET_CONFIG_OPT(camera, lucid, device_link_throughput_limit); SET_CONFIG_OPT(camera, lucid, gamma_enable); - SET_CONFIG_OPT(camera, lucid, gamma); // bruh + SET_CONFIG_OPT(camera, lucid, gamma); // bruh SET_CONFIG_OPT(camera, lucid, gain_auto); SET_CONFIG_OPT(camera, lucid, gain_auto_upper_limit); SET_CONFIG_OPT(camera, lucid, gain_auto_lower_limit);