From d294f9f56eb026505ee66fce2d84752e93649ffc Mon Sep 17 00:00:00 2001 From: Askew <87111319+AskewParity@users.noreply.github.com> Date: Mon, 10 Jun 2024 18:06:06 -0700 Subject: [PATCH] Chore/spaceout waypoints (#199) * refactor config * beautiful, beautiful macros * fix lint & add camera vision config option * dubins is now in config * removed all instances of constants --------- Co-authored-by: Tyler Lentz --- configs/config.json | 34 +++-- configs/dev-config.json | 34 +++-- include/camera/interface.hpp | 1 + include/pathing/static.hpp | 65 ++++---- include/pathing/tree.hpp | 3 +- include/utilities/constants.hpp | 4 - include/utilities/datatypes.hpp | 73 --------- include/utilities/obc_config.hpp | 174 +++++++++++++++++++--- include/utilities/obc_config_macros.hpp | 106 +++++++++++++ src/core/obc.cpp | 10 +- src/network/gcs_routes.cpp | 2 +- src/pathing/environment.cpp | 74 ++++----- src/pathing/static.cpp | 100 ++++++++----- src/pathing/tree.cpp | 13 +- src/utilities/obc_config.cpp | 163 ++++++++++---------- tests/integration/airdrop_approach.cpp | 15 +- tests/integration/camera_lucid.cpp | 4 +- tests/integration/camera_mock.cpp | 4 +- tests/integration/coverage_pathing.cpp | 22 ++- tests/integration/coverage_pathing_2.cpp | 20 ++- tests/integration/path_planning.cpp | 23 ++- tests/integration/pathing_integration.cpp | 2 +- 22 files changed, 583 insertions(+), 363 deletions(-) create mode 100644 include/utilities/obc_config_macros.hpp diff --git a/configs/config.json b/configs/config.json index 70848352..054d6a65 100644 --- a/configs/config.json +++ b/configs/config.json @@ -14,23 +14,33 @@ "altitude_m": 30.0 }, "pathing": { - "_comment": "Point Fetch Method 2: NEAREST", - "_comment2": "Drop Method 1: UNGUIDED", + "dubins": { + "turning_radius": 30.0, + "point_separation": 20.0 + }, "rrt": { - "iterations_per_waypoint": 200, - "rewire_radius": 200.0, + "iterations_per_waypoint": 512, + "rewire_radius": 256.0, "optimize": true, - "point_fetch_methods": 2, + "point_fetch_method": "nearest", "allowed_to_skip_waypoints": false }, "coverage": { - "coverage_altitude_m": 30.0, - "optimize": true, - "vertical": false, - "one_way": false + "altitude_m": 30.0, + "camera_vision_m": 20, + "method": "hover", + "hover": { + "pictures_per_stop": 1, + "hover_time_s": 5 + }, + "forward": { + "optimize": true, + "vertical": false, + "one_way": false + } }, "approach": { - "drop_method": 1, + "drop_method": "unguided", "drop_angle_rad": 2.52134317, "drop_altitude_m": 26.0, "guided_drop_distance_m": 50.0, @@ -50,10 +60,10 @@ "images_dir": "/obcpp/tests/integration/images/saliency/" }, "lucid": { - "sensor_shuttle_mode": "Rolling", + "sensor_shutter_mode": "Rolling", "acquisition_frame_rate_enable": true, - "target_brightness": 70, + "target_brightness": 20, "exposure_auto": "Continuous", "exposure_time": 3000, "exposure_auto_damping": 1, diff --git a/configs/dev-config.json b/configs/dev-config.json index 76bb7a37..d981e940 100644 --- a/configs/dev-config.json +++ b/configs/dev-config.json @@ -14,23 +14,33 @@ "altitude_m": 30.0 }, "pathing": { - "_comment": "Point Fetch Method 2: NEAREST", - "_comment2": "Drop Method 1: UNGUIDED", + "dubins": { + "turning_radius": 30.0, + "point_separation": 20.0 + }, "rrt": { - "iterations_per_waypoint": 200, - "rewire_radius": 200.0, + "iterations_per_waypoint": 512, + "rewire_radius": 256.0, "optimize": true, - "point_fetch_methods": 2, + "point_fetch_method": "nearest", "allowed_to_skip_waypoints": false }, "coverage": { - "coverage_altitude_m": 30.0, - "optimize": true, - "vertical": false, - "one_way": false + "altitude_m": 30.0, + "camera_vision_m": 20, + "method": "hover", + "hover": { + "pictures_per_stop": 1, + "hover_time_s": 5 + }, + "forward": { + "optimize": true, + "vertical": false, + "one_way": false + } }, "approach": { - "drop_method": 1, + "drop_method": "unguided", "drop_angle_rad": 2.52134317, "drop_altitude_m": 26.0, "guided_drop_distance_m": 50.0, @@ -50,10 +60,10 @@ "images_dir": "/workspaces/obcpp/tests/integration/images/saliency/" }, "lucid": { - "sensor_shuttle_mode": "Rolling", + "sensor_shutter_mode": "Rolling", "acquisition_frame_rate_enable": true, - "target_brightness": 70, + "target_brightness": 20, "exposure_auto": "Continuous", "exposure_time": 3000, "exposure_auto_damping": 1, diff --git a/include/camera/interface.hpp b/include/camera/interface.hpp index 517e8e6d..52567a13 100644 --- a/include/camera/interface.hpp +++ b/include/camera/interface.hpp @@ -15,6 +15,7 @@ #include "network/mavlink.hpp" #include "utilities/datatypes.hpp" +#include "utilities/obc_config.hpp" using json = nlohmann::json; using Mat = cv::Mat; diff --git a/include/pathing/static.hpp b/include/pathing/static.hpp index 50759f4a..e9f680fc 100644 --- a/include/pathing/static.hpp +++ b/include/pathing/static.hpp @@ -23,19 +23,10 @@ class RRT { public: RRT(RRTPoint start, std::vector goals, double search_radius, Polygon bounds, - std::vector obstacles = {}, std::vector angles = {}, - RRTConfig config = {.iterations_per_waypoint = ITERATIONS_PER_WAYPOINT, - .rewire_radius = REWIRE_RADIUS, - .optimize = false, - .point_fetch_method = POINT_FETCH_METHODS::NEAREST, - .allowed_to_skip_waypoints = false}); + const OBCConfig &config, std::vector obstacles = {}, + std::vector angles = {}); RRT(RRTPoint start, std::vector goals, double search_radius, Environment airspace, - std::vector angles = {}, - RRTConfig config = {.iterations_per_waypoint = ITERATIONS_PER_WAYPOINT, - .rewire_radius = REWIRE_RADIUS, - .optimize = false, - .point_fetch_method = POINT_FETCH_METHODS::NONE, - .allowed_to_skip_waypoints = false}); + const OBCConfig &config, std::vector angles = {}); /** * RRT(-star) algorithm @@ -190,15 +181,16 @@ class RRT { * Limitations * - Cannot path through non-convex shapes * - Does not check if path is inbounds or not + * + * Notes: + * - this implementation is for fixed wing planes, which is not currently being used. However, + * it is kept here because it is very possible we will eventually switch back to it. */ -class CoveragePathing { +class ForwardCoveragePathing { public: - CoveragePathing(const RRTPoint &start, double scan_radius, Polygon bounds, Polygon airdrop_zone, - std::vector obstacles = {}, - AirdropSearchConfig config = {.coverage_altitude_m = 30.0, - .optimize = false, - .vertical = false, - .one_way = false}); + ForwardCoveragePathing(const RRTPoint &start, double scan_radius, Polygon bounds, + Polygon airdrop_zone, const OBCConfig &config, + std::vector obstacles = {}); /** * Generates a path of parallel lines to cover a given area @@ -236,21 +228,32 @@ class CoveragePathing { const RRTPoint start; // start location (doesn't have to be near polygon) const Environment airspace; // information aobut the airspace const Dubins dubins; // dubins object to generate paths - const AirdropSearchConfig config; + const AirdropCoverageConfig config; +}; + +/** + * Class that performs coverage pathing over a given search area, given that the plane has + * hovering capabilities and that we want to be taking pictures while hovering over the zone. + * + * This outputs a series of XYZ Coordinates which represent a points at which the plane + * should hover and take a picture. + */ +class HoverCoveragePathing { + public: + HoverCoveragePathing(Polygon flight_bounds, Polygon drop_zone); + + std::vector run(); + + private: + Polygon flight_bounds; + Polygon drop_zone; }; class AirdropApproachPathing { public: AirdropApproachPathing(const RRTPoint &start, const XYZCoord &goal, RRTPoint wind, - Polygon bounds, std::vector obstacles = {}, - AirdropApproachConfig config = { - .drop_method = UNGUIDED, - .bottle_ids = {1, 2, 3, 4, 5}, - .drop_angle_rad = DROP_ANGLE_RAD, - // .drop_angle_rad = M_PI * 3 / 4, - .drop_altitude_m = DROP_ALTITUDE_M, - .guided_drop_distance_m = GUIDED_DROP_DISTANCE_M, - .unguided_drop_distance_m = UNGUIDED_DROP_DISTANCE_M}); + Polygon bounds, const OBCConfig &config, + std::vector obstacles = {}); /** * Generates a path to the drop location * @@ -268,11 +271,13 @@ class AirdropApproachPathing { const RRTPoint start; const Environment airspace; const Dubins dubins; - const AirdropApproachConfig config; + const OBCConfig config; RRTPoint wind; }; std::vector generateInitialPath(std::shared_ptr state); +std::vector generateSearchPath(std::shared_ptr state); + #endif // INCLUDE_PATHING_STATIC_HPP_ diff --git a/include/pathing/tree.hpp b/include/pathing/tree.hpp index 920b3f1e..f834877b 100644 --- a/include/pathing/tree.hpp +++ b/include/pathing/tree.hpp @@ -10,6 +10,7 @@ #include "pathing/dubins.hpp" #include "pathing/environment.hpp" #include "utilities/datatypes.hpp" +#include "utilities/obc_config.hpp" #include "utilities/rng.hpp" class RRTNode; @@ -196,7 +197,7 @@ class RRTTree { * @return ==> mininum sorted list of pairs of */ std::vector> pathingOptions( - const RRTPoint& end, POINT_FETCH_METHODS path_option = POINT_FETCH_METHODS::NONE, + const RRTPoint& end, PointFetchMethod::Enum path_option = PointFetchMethod::Enum::NONE, int quantity_options = MAX_DUBINS_OPTIONS_TO_PARSE) const; /** DOES RRT* for the program diff --git a/include/utilities/constants.hpp b/include/utilities/constants.hpp index def99aa8..a28ea79d 100644 --- a/include/utilities/constants.hpp +++ b/include/utilities/constants.hpp @@ -14,10 +14,6 @@ const size_t MAX_CV_PIPELINES = 5; const double TWO_PI = 2 * M_PI; const double HALF_PI = M_PI / 2; -// FROM OBC PYTHON -const double TURNING_RADIUS = 30.0; -const double POINT_SEPARATION = 10.0; - // RRT CONSTANTS const int ITERATIONS_PER_WAYPOINT = 256; // number of times RRT is ran per waypoint const double SEARCH_RADIUS = diff --git a/include/utilities/datatypes.hpp b/include/utilities/datatypes.hpp index e0b2b8bc..8cf961e3 100644 --- a/include/utilities/datatypes.hpp +++ b/include/utilities/datatypes.hpp @@ -92,79 +92,6 @@ using Polyline = std::vector; using GPSProtoVec = google::protobuf::RepeatedPtrField; -enum POINT_FETCH_METHODS { - NONE, // check RRT against every node (path optimal, but incredibly slow) - RANDOM, // check ~k randomly sampled nodes from the tree. - NEAREST // check ~$p$ nodes closest to the sampled node (best performance/time ratio from - // rudimentary testing) -}; - -struct RRTConfig { - int iterations_per_waypoint; // number of iterations run between two waypoints - double rewire_radius; // maximum distance from sampled point to optimize during RRT* - bool optimize; // run RRT* if true - POINT_FETCH_METHODS point_fetch_method; - bool allowed_to_skip_waypoints; // if true, will skip waypoints if it can not connect after 1 - // RRT iteration -}; - -struct AirdropSearchConfig { - double coverage_altitude_m; - bool optimize; // whether to ignore the config below and run all ways. - bool vertical; // if true, will search in vertical lines - bool one_way; // if true, path returned will only be in 1 direction -}; - -struct AirdropApproachConfig { - drop_mode drop_method; - std::unordered_set bottle_ids; - double drop_angle_rad; - double drop_altitude_m; - double guided_drop_distance_m; - double unguided_drop_distance_m; -}; -struct CameraConfig { - // either "mock" or "lucid" - std::string type; - // directory to save images to - std::string save_dir; - struct { - // directory to randomly pick images from - // for the mock camera - std::string images_dir; - } mock; - // All comments will reference the nodes for the Triton 200s - // https://support.thinklucid.com/triton-tri200s/ - struct { - // Image Format Control (https://support.thinklucid.com/triton-tri200s/#2976) - std::string sensor_shutter_mode; // Either "Rolling" or "GlobalReset" - - // Acquisition Control (https://support.thinklucid.com/triton-tri200s/#2934) - bool acquisition_frame_rate_enable; - int64_t target_brightness; - std::string exposure_auto; // either "Continuous" or "Off" - double exposure_time; // manual exposure time. only applies when exposure_auto is "Off" - std::string exposure_auto_algorithm; // either "Median" or "Mean" - double exposure_auto_damping; - double exposure_auto_upper_limit; - double exposure_auto_lower_limit; - - // Stream settings - bool stream_auto_negotiate_packet_size; - bool stream_packet_resend_enable; - - // Device Control (https://support.thinklucid.com/triton-tri200s/#2959) - std::string device_link_throughput_limit_mode; // Either "On" or "Off" - int64_t device_link_throughput_limit; // for Triton 200S: max 125,000,000 min 31,250,000 - - // Analog Control (https://support.thinklucid.com/triton-tri200s/#2953) - bool gamma_enable; - double gamma; - std::string gain_auto; // either "Continuous" or "Off" - double gain_auto_upper_limit; - double gain_auto_lower_limit; - } lucid; -}; #endif // INCLUDE_UTILITIES_DATATYPES_HPP_ diff --git a/include/utilities/obc_config.hpp b/include/utilities/obc_config.hpp index a476121f..42989e4d 100644 --- a/include/utilities/obc_config.hpp +++ b/include/utilities/obc_config.hpp @@ -1,39 +1,169 @@ #ifndef INCLUDE_UTILITIES_OBC_CONFIG_HPP_ #define INCLUDE_UTILITIES_OBC_CONFIG_HPP_ +#include #include +#include +#include +#include #include "udp_squared/internal/enum.h" #include "utilities/constants.hpp" #include "utilities/datatypes.hpp" -struct OBCConfig { - struct { - std::string dir; - } logging; +#define CONFIG_VARIANT_MAPPING_T(enum_type) \ + const std::initializer_list> + +struct LoggingConfig { + std::string dir; +}; +struct NetworkConfig { struct { - struct { - int port; - } gcs; - struct { - std::string connect; - } mavlink; - } network; + int port; + } gcs; + struct { + std::string connect; + } mavlink; +}; + +struct TakeoffConfig { + float altitude_m; +}; + +struct CVConfig { + std::string matching_model_dir; + std::string segmentation_model_dir; + std::string saliency_model_dir; +}; + +namespace PointFetchMethod { + enum class Enum { + NONE, // check RRT against every node (path optimal, but incredibly slow) + RANDOM, // check ~k randomly sampled nodes from the tree. + NEAREST // check ~$p$ nodes closest to the sampled node (best performance/time ratio from + // rudimentary testing) + }; + CONFIG_VARIANT_MAPPING_T(Enum) MAPPINGS = { + {"none", Enum::NONE}, {"random", Enum::RANDOM}, {"nearest", Enum::NEAREST} + }; +}; // namespace PointFetchMethod + +struct DubinsConfig { + double turning_radius; + double point_separation; +}; + +struct RRTConfig { + int iterations_per_waypoint; // number of iterations run between two waypoints + double rewire_radius; // maximum distance from sampled point to optimize during RRT* + bool optimize; // run RRT* if true + PointFetchMethod::Enum point_fetch_method; + bool allowed_to_skip_waypoints; // if true, will skip waypoints if it can not connect after 1 + // RRT iteration +}; + +namespace AirdropCoverageMethod { + enum class Enum { + HOVER, + FORWARD + }; + 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 { - float altitude_m; - } takeoff; + std::size_t pictures_per_stop; + std::time_t hover_time_s; + } hover; + struct { + bool optimize; // whether to ignore the config below and run all ways. + bool vertical; // if true, will search in vertical lines + bool one_way; // if true, path returned will only be in 1 direction + } forward; +}; + +namespace AirdropDropMethod { + enum class Enum { + GUIDED, + UNGUIDED + }; + CONFIG_VARIANT_MAPPING_T(Enum) MAPPINGS = { + {"guided", Enum::GUIDED}, {"unguided", Enum::UNGUIDED} + }; +}; + +struct AirdropApproachConfig { + AirdropDropMethod::Enum drop_method; + std::unordered_set bottle_ids; + double drop_angle_rad; + double drop_altitude_m; + double guided_drop_distance_m; + double unguided_drop_distance_m; +}; + +struct PathingConfig { + DubinsConfig dubins; + RRTConfig rrt; + AirdropCoverageConfig coverage; + AirdropApproachConfig approach; +}; +struct CameraConfig { + // either "mock" or "lucid" + std::string type; + // directory to save images to + std::string save_dir; struct { - std::string matching_model_dir; - std::string segmentation_model_dir; - std::string saliency_model_dir; - } cv; - - RRTConfig rrt_config; - AirdropSearchConfig coverage_pathing_config; - AirdropApproachConfig airdrop_pathing_config; - CameraConfig camera_config; + // directory to randomly pick images from + // for the mock camera + std::string images_dir; + } mock; + // All comments will reference the nodes for the Triton 200s + // https://support.thinklucid.com/triton-tri200s/ + struct { + // Image Format Control (https://support.thinklucid.com/triton-tri200s/#2976) + std::string sensor_shutter_mode; // Either "Rolling" or "GlobalReset" + + // Acquisition Control (https://support.thinklucid.com/triton-tri200s/#2934) + bool acquisition_frame_rate_enable; + int64_t target_brightness; + std::string exposure_auto; // either "Continuous" or "Off" + double exposure_time; // manual exposure time. only applies when exposure_auto is "Off" + std::string exposure_auto_algorithm; // either "Median" or "Mean" + double exposure_auto_damping; + double exposure_auto_upper_limit; + double exposure_auto_lower_limit; + + // Stream settings + bool stream_auto_negotiate_packet_size; + bool stream_packet_resend_enable; + + // Device Control (https://support.thinklucid.com/triton-tri200s/#2959) + std::string device_link_throughput_limit_mode; // Either "On" or "Off" + int64_t device_link_throughput_limit; // for Triton 200S: max 125,000,000 min 31,250,000 + + // Analog Control (https://support.thinklucid.com/triton-tri200s/#2953) + bool gamma_enable; + double gamma; + std::string gain_auto; // either "Continuous" or "Off" + double gain_auto_upper_limit; + double gain_auto_lower_limit; + } lucid; +}; + +struct OBCConfig { + LoggingConfig logging; + NetworkConfig network; + TakeoffConfig takeoff; + CVConfig cv; + DubinsConfig dubins; + PathingConfig pathing; + CameraConfig camera; // Load user specified config json, or make a new one OBCConfig(int argc, char* argv[]); diff --git a/include/utilities/obc_config_macros.hpp b/include/utilities/obc_config_macros.hpp new file mode 100644 index 00000000..aa8a5bf7 --- /dev/null +++ b/include/utilities/obc_config_macros.hpp @@ -0,0 +1,106 @@ +#ifndef INCLUDE_UTILITIES_OBC_CONFIG_MACROS_HPP_ +#define INCLUDE_UTILITIES_OBC_CONFIG_MACROS_HPP_ + +#include + +/* + ( ) /\ _ ( + \ | ( \ ( \.( ) _____ + \ \ \ ` ` ) \ ( ___ / _ \ + (_` \+ . x ( .\ \/ \____-----------/ (o) \_ +- .- \+ ; ( O \____ + ) \_____________ ` \ / +(__ +- .( -'.- <. - _ VVVVVVV VV V\ \/ +(_____ ._._: <_ - <- _ (-- _AAAAAAA__A_/ | + . /./.+- . .- / +-- - . \______________//_ \_______ + (__ ' /x / x _/ ( \___' \ / + , x / ( ' . / . / | \ / + / / _/ / + / \/ + ' (__/ / \ + HERE BE DRAGONS + + Don't edit these unless you know what you are doing! + + You shouldn't ever need to look in here. Ever. It should just work. If it is not working + then you are doing something wrong... probably. +*/ + +// dont worry about it... +// 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 + +// 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 +#define HANDLE_JSON_EXCEPTION(expr) \ + try { \ + return expr; \ + } catch (nlohmann::json::exception ex) { \ + LOG_F(FATAL, "%s", ex.what()); \ + 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 +#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) +#define PARSE_CONFIG_4(p1, p2, p3, p4) configs.at(#p1).at(#p2).at(#p3).at(#p4) +#define PARSE_CONFIG_5(p1, p2, p3, p4, p5) configs.at(#p1).at(#p2).at(#p3).at(#p4).at(#p5) + +// wrapper around accessing the configs json and handling exceptions with useful error messages +// yes this is an inline lambda function that is instantly being called +// yes there is probably a cleaner way to do this +// and yes this works +#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__)) \ + }() + +// look in the code below for an example on how to use this function +// basically handles parsing a string and converting it to a valid enum value +// panics if the string value is invalid +template +constexpr T PARSE_VARIANT(CONFIG_VARIANT_MAPPING_T(T) mapping, std::string input) { + for (const auto& [str_val, enum_val] : mapping) { + if (input == str_val) { + return enum_val; + } + } + 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 +} + +// and then at the very end we prevent having to duplicate typing +// the struct config option and the json accessing syntax... + +#define SET_CONFIG_OPT_5(p1, p2, p3, p4, p5) \ + this->p1.p2.p3.p4.p5 = PARSE(p1, p2, p3, p4, p5) +#define SET_CONFIG_OPT_4(p1, p2, p3, p4) \ + this->p1.p2.p3.p4 = PARSE(p1, p2, p3, p4) +#define SET_CONFIG_OPT_3(p1, p2, p3) \ + this->p1.p2.p3 = PARSE(p1, p2, p3) +#define SET_CONFIG_OPT_2(p1, p2) \ + this->p1.p2 = PARSE(p1, p2) +#define SET_CONFIG_OPT(...) \ + GET_MACRO_5(__VA_ARGS__, SET_CONFIG_OPT_5, SET_CONFIG_OPT_4, \ + SET_CONFIG_OPT_3, SET_CONFIG_OPT_2) (__VA_ARGS__) + +// then we finally repeat everything for the variants... +#define SET_CONFIG_OPT_5_VARIANT(mapping, p1, p2, p3, p4, p5) \ + this->p1.p2.p3.p4.p5 = PARSE_VARIANT(mapping, PARSE(p1, p2, p3, p4, p5)) +#define SET_CONFIG_OPT_4_VARIANT(mapping, p1, p2, p3, p4) \ + this->p1.p2.p3.p4 = PARSE_VARIANT(mapping, PARSE(p1, p2, p3, p4)) +#define SET_CONFIG_OPT_3_VARIANT(mapping, p1, p2, p3) \ + this->p1.p2.p3 = PARSE_VARIANT(mapping, PARSE(p1, p2, p3)) +#define SET_CONFIG_OPT_2_VARIANT(mapping, p1, p2) \ + this->p1.p2 = PARSE_VARIANT(mapping, PARSE(p1, p2)) +#define SET_CONFIG_OPT_VARIANT(mapping, ...) \ + 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_ diff --git a/src/core/obc.cpp b/src/core/obc.cpp index 5ab403f5..b04110c2 100644 --- a/src/core/obc.cpp +++ b/src/core/obc.cpp @@ -35,17 +35,17 @@ OBC::OBC(OBCConfig config) { {this->connectMavlink(config.network.mavlink.connect);}); this->connectAirdropThread = std::thread([this]{this->connectAirdrop();}); - if (this->state->config.camera_config.type == "mock") { - this->state->setCamera(std::make_shared(this->state->config.camera_config)); - } else if (this->state->config.camera_config.type == "lucid") { + if (this->state->config.camera.type == "mock") { + this->state->setCamera(std::make_shared(this->state->config.camera)); + } else if (this->state->config.camera.type == "lucid") { #ifdef ARENA_SDK_INSTALLED - this->state->setCamera(std::make_shared(this->state->config.camera_config)); //NOLINT + this->state->setCamera(std::make_shared(this->state->config.camera)); //NOLINT #else LOG_F(FATAL, "Attempting to create Lucid Camera without having ArenaSDK installed."); #endif } else { // default to mock if it's neither "mock" or "lucid" - this->state->setCamera(std::make_shared(this->state->config.camera_config)); + this->state->setCamera(std::make_shared(this->state->config.camera)); } } diff --git a/src/network/gcs_routes.cpp b/src/network/gcs_routes.cpp index 5e64c8eb..d567a694 100644 --- a/src/network/gcs_routes.cpp +++ b/src/network/gcs_routes.cpp @@ -189,7 +189,7 @@ DEF_GCS_HANDLE(Get, camera, capture) { std::optional telemetry = image->TELEMETRY; try { - std::filesystem::path save_dir = state->config.camera_config.save_dir; + std::filesystem::path save_dir = state->config.camera.save_dir; std::filesystem::path img_filepath = save_dir / (std::to_string(image->TIMESTAMP) + std::string(".jpg")); std::filesystem::path json_filepath = diff --git a/src/pathing/environment.cpp b/src/pathing/environment.cpp index 32ccb216..ae4e8012 100644 --- a/src/pathing/environment.cpp +++ b/src/pathing/environment.cpp @@ -74,43 +74,43 @@ bool Environment::isPathInBounds(const std::vector& path) const { return true; } -bool Environment::isPathInBoundsAdv(const std::vector& path, - const RRTOption& option) const { - if (!option.has_straight) { - return isPathInBounds(path); - } - - // finds the last point on the first curve, and the first point on the second curve - // does this using the option, using arclength and the point separation - const int first_curve_end = - std::abs(option.dubins_path.beta_0) * TURNING_RADIUS / POINT_SEPARATION + 1; - const int second_curve_start = - path.size() - std::abs(option.dubins_path.beta_2) * TURNING_RADIUS / POINT_SEPARATION; - - // sanity check - if (first_curve_end >= second_curve_start) { - return isPathInBounds(path); - } - - if (!isLineInBounds(path[first_curve_end], path[second_curve_start])) { - return false; - } - - // checks the points manually in the curve - for (int i = 0; i <= first_curve_end; i++) { - if (!isPointInBounds(path[i])) { - return false; - } - } - - for (int i = second_curve_start; i < path.size(); i++) { - if (!isPointInBounds(path[i])) { - return false; - } - } - - return true; -} +// bool Environment::isPathInBoundsAdv(const std::vector& path, +// const RRTOption& option) const { +// if (!option.has_straight) { +// return isPathInBounds(path); +// } + +// // finds the last point on the first curve, and the first point on the second curve +// // does this using the option, using arclength and the point separation +// const int first_curve_end = +// std::abs(option.dubins_path.beta_0) * TURNING_RADIUS / POINT_SEPARATION + 1; +// const int second_curve_start = +// path.size() - std::abs(option.dubins_path.beta_2) * TURNING_RADIUS / POINT_SEPARATION; + +// // sanity check +// if (first_curve_end >= second_curve_start) { +// return isPathInBounds(path); +// } + +// if (!isLineInBounds(path[first_curve_end], path[second_curve_start])) { +// return false; +// } + +// // checks the points manually in the curve +// for (int i = 0; i <= first_curve_end; i++) { +// if (!isPointInBounds(path[i])) { +// return false; +// } +// } + +// for (int i = second_curve_start; i < path.size(); i++) { +// if (!isPointInBounds(path[i])) { +// return false; +// } +// } + +// return true; +// } const XYZCoord& Environment::getGoal() const { return goals[goals_found]; } diff --git a/src/pathing/static.cpp b/src/pathing/static.cpp index fe66a502..a02c2dee 100644 --- a/src/pathing/static.cpp +++ b/src/pathing/static.cpp @@ -16,28 +16,30 @@ #include "pathing/tree.hpp" #include "utilities/constants.hpp" #include "utilities/datatypes.hpp" +#include "utilities/obc_config.hpp" #include "utilities/rng.hpp" RRT::RRT(RRTPoint start, std::vector goals, double search_radius, Polygon bounds, - std::vector obstacles, std::vector angles, RRTConfig config) - : iterations_per_waypoint(config.iterations_per_waypoint), + 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.rewire_radius), + rewire_radius(config.pathing.rrt.rewire_radius), tree(start, Environment(bounds, {}, goals, obstacles), - Dubins(TURNING_RADIUS, POINT_SEPARATION)), - config(config) { + Dubins(config.pathing.dubins.turning_radius, config.pathing.dubins.point_separation)), + config(config.pathing.rrt) { if (angles.size() != 0) { this->angles = angles; } } RRT::RRT(RRTPoint start, std::vector goals, double search_radius, Environment airspace, - std::vector angles, RRTConfig config) - : iterations_per_waypoint(config.iterations_per_waypoint), + const OBCConfig &config, std::vector angles) + : iterations_per_waypoint(config.pathing.rrt.iterations_per_waypoint), search_radius(search_radius), - rewire_radius(config.rewire_radius), - tree(start, airspace, Dubins(TURNING_RADIUS, POINT_SEPARATION)), - config(config) { + rewire_radius(config.pathing.rrt.rewire_radius), + tree(start, airspace, + Dubins(config.pathing.dubins.turning_radius, config.pathing.dubins.point_separation)), + config(config.pathing.rrt) { if (angles.size() != 0) { this->angles = angles; } @@ -177,7 +179,7 @@ std::vector>> RRT::getOption for (const RRTPoint &goal : goal_points) { const std::vector> &options = // For now, we use optimal pathing - tree.pathingOptions(goal, POINT_FETCH_METHODS::NONE, NUMBER_OPTIONS_EACH); + tree.pathingOptions(goal, PointFetchMethod::Enum::NONE, NUMBER_OPTIONS_EACH); for (const auto &[node, option] : options) { all_options.push_back({goal, {node, option}}); @@ -280,23 +282,24 @@ RRTNode *RRT::parseOptions(const std::vector> &o void RRT::optimizeTree(RRTNode *sample) { tree.RRTStar(sample, rewire_radius); } -CoveragePathing::CoveragePathing(const RRTPoint &start, double scan_radius, Polygon bounds, - Polygon airdrop_zone, std::vector obstacles, - AirdropSearchConfig config) +ForwardCoveragePathing::ForwardCoveragePathing(const RRTPoint &start, double scan_radius, + Polygon bounds, Polygon airdrop_zone, + const OBCConfig &config, + std::vector obstacles) : start(start), scan_radius(scan_radius), airspace(Environment(bounds, airdrop_zone, {}, obstacles)), - dubins(Dubins(TURNING_RADIUS, POINT_SEPARATION)), - config(config) {} + dubins(Dubins(config.pathing.dubins.turning_radius, config.pathing.dubins.point_separation)), + config(config.pathing.coverage) {} -std::vector CoveragePathing::run() const { - return config.optimize ? coverageOptimal() : coverageDefault(); +std::vector ForwardCoveragePathing::run() const { + return config.forward.optimize ? coverageOptimal() : coverageDefault(); } -std::vector CoveragePathing::coverageDefault() const { +std::vector ForwardCoveragePathing::coverageDefault() const { // generates the endpoints for the lines (including headings) std::vector waypoints = - airspace.getAirdropWaypoints(scan_radius, config.one_way, config.vertical); + airspace.getAirdropWaypoints(scan_radius, config.forward.one_way, config.forward.vertical); waypoints.emplace(waypoints.begin(), start); // generates the path connecting the q @@ -308,7 +311,7 @@ std::vector CoveragePathing::coverageDefault() const { return generatePath(dubins_options, waypoints); } -std::vector CoveragePathing::coverageOptimal() const { +std::vector ForwardCoveragePathing::coverageOptimal() const { /* * The order of paths * [0] - alt, vertical @@ -361,13 +364,13 @@ std::vector CoveragePathing::coverageOptimal() const { return generatePath(dubins_paths[best_path_idx], waypoints); } -std::vector CoveragePathing::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 double height = waypoints[0].coord.z; - double height_difference = config.coverage_altitude_m - waypoints[0].coord.z; + double height_difference = config.altitude_m - waypoints[0].coord.z; std::vector path_coordinates = dubins.generatePoints( waypoints[0], waypoints[1], dubins_options[0].dubins_path, dubins_options[0].has_straight); @@ -388,7 +391,7 @@ std::vector CoveragePathing::generatePath(const std::vector dubins_options[i].has_straight); for (XYZCoord &coord : path_coordinates) { - coord.z = config.coverage_altitude_m; + coord.z = config.altitude_m; } path.insert(path.end(), path_coordinates.begin() + 1, path_coordinates.end()); @@ -397,29 +400,38 @@ std::vector CoveragePathing::generatePath(const std::vector return path; } +HoverCoveragePathing::HoverCoveragePathing(Polygon flight_bounds, Polygon drop_zone) + : flight_bounds{flight_bounds}, drop_zone{drop_zone} {} + +std::vector HoverCoveragePathing::run() { return {}; } + AirdropApproachPathing::AirdropApproachPathing(const RRTPoint &start, const XYZCoord &goal, RRTPoint wind, Polygon bounds, - std::vector obstacles, - AirdropApproachConfig config) + const OBCConfig &config, + std::vector obstacles) : start(start), goal(goal), wind(wind), airspace(Environment(bounds, {}, {goal}, obstacles)), - dubins(Dubins(TURNING_RADIUS, POINT_SEPARATION)), + dubins(Dubins(config.pathing.dubins.turning_radius, config.pathing.dubins.point_separation)), config(config) {} std::vector AirdropApproachPathing::run() const { RRTPoint drop_vector = getDropLocation(); - RRT rrt(start, {drop_vector.coord}, SEARCH_RADIUS, airspace, {drop_vector.psi}); + RRT rrt(start, {drop_vector.coord}, SEARCH_RADIUS, airspace, config, {drop_vector.psi}); rrt.run(); return rrt.getPointsToGoal(); } RRTPoint AirdropApproachPathing::getDropLocation() const { - double drop_angle = config.drop_angle_rad; - double drop_distance = config.drop_method == GUIDED ? config.unguided_drop_distance_m - : config.guided_drop_distance_m; + double drop_angle = config.pathing.approach.drop_angle_rad; + double drop_distance = 0.0f; + if (config.pathing.approach.drop_method == AirdropDropMethod::Enum::GUIDED) { + drop_distance = config.pathing.approach.guided_drop_distance_m; + } else { + drop_distance = config.pathing.approach.unguided_drop_distance_m; + } XYZCoord drop_offset(drop_distance * std::cos(drop_angle), drop_distance * std::sin(drop_angle), 0); @@ -428,7 +440,8 @@ RRTPoint AirdropApproachPathing::getDropLocation() const { XYZCoord wind_offset(wind_strength_coef * std::cos(wind.psi), wind_strength_coef * std::sin(wind.psi), 0); XYZCoord drop_location(goal.x + drop_offset.x + wind_offset.x, - goal.y + drop_offset.y + wind_offset.y, config.drop_altitude_m); + goal.y + drop_offset.y + wind_offset.y, + config.pathing.approach.drop_altitude_m); // gets the angle between the drop_location and the goal double angle = std::atan2(goal.y - drop_location.y, goal.x - drop_location.x); @@ -460,8 +473,8 @@ std::vector generateInitialPath(std::shared_ptr state) { RRTPoint start(state->mission_params.getWaypoints().front(), init_angle); start.coord.z = state->config.takeoff.altitude_m; - RRT rrt(start, goals, SEARCH_RADIUS, state->mission_params.getFlightBoundary(), {}, {}, - state->config.rrt_config); + RRT rrt(start, goals, SEARCH_RADIUS, state->mission_params.getFlightBoundary(), state->config, + {}, {}); rrt.run(); @@ -470,8 +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) { + 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/pathing/tree.cpp b/src/pathing/tree.cpp index 4c8392a0..f330f70e 100644 --- a/src/pathing/tree.cpp +++ b/src/pathing/tree.cpp @@ -9,6 +9,7 @@ #include "utilities/datatypes.hpp" #include "utilities/logging.hpp" #include "utilities/rng.hpp" +#include "utilities/obc_config.hpp" RRTNode::RRTNode(const RRTPoint& point, double cost, double path_length, const std::vector path) @@ -94,8 +95,8 @@ RRTTree::RRTTree(RRTPoint root_point, Environment airspace, Dubins dubins) RRTTree::~RRTTree() { delete root; } bool RRTTree::validatePath(const std::vector& path, const RRTOption& option) const { - // return airspace.isPathInBounds(path); - return airspace.isPathInBoundsAdv(path, option); + return airspace.isPathInBounds(path); + // return airspace.isPathInBoundsAdv(path, option); } RRTNode* RRTTree::generateNode(RRTNode* anchor_node, const RRTPoint& new_point, @@ -286,20 +287,20 @@ RRTPoint RRTTree::getRandomPoint(double search_radius) const { TODO - investigate whether a max heap is better or worse */ std::vector> RRTTree::pathingOptions( - const RRTPoint& end, POINT_FETCH_METHODS point_fetch_method, int quantity_options) const { + const RRTPoint& end, PointFetchMethod::Enum point_fetch_method, int quantity_options) const { // fills the options list with valid values std::vector> options; switch (point_fetch_method) { - case POINT_FETCH_METHODS::RANDOM: { + case PointFetchMethod::Enum::RANDOM: { const std::vector& nodes = getKRandomNodes(K_RANDOM_NODES); fillOptionsNodes(options, nodes, end); } break; - case POINT_FETCH_METHODS::NEAREST: { + case PointFetchMethod::Enum::NEAREST: { const std::vector& nodes = getKClosestNodes(end, K_CLOESEST_NODES); fillOptionsNodes(options, nodes, end); } break; - case POINT_FETCH_METHODS::NONE: + case PointFetchMethod::Enum::NONE: fillOptions(options, current_head, end); break; default: diff --git a/src/utilities/obc_config.cpp b/src/utilities/obc_config.cpp index cb446ad0..9e7514cd 100644 --- a/src/utilities/obc_config.cpp +++ b/src/utilities/obc_config.cpp @@ -10,96 +10,85 @@ #include "utilities/constants.hpp" #include "utilities/datatypes.hpp" #include "utilities/logging.hpp" - -using json = nlohmann::json; +#include "utilities/obc_config_macros.hpp" OBCConfig::OBCConfig(int argc, char* argv[]) { // If config-json name is passed in - if (argc > 1) { - // Load in json file - std::ifstream configStream(argv[1]); - if (!configStream.is_open()) { - throw std::invalid_argument(std::string("Invalid path to config file: ") + - std::string(argv[1])); - } - json configs = json::parse(configStream); - - // Set configs - this->logging.dir = configs["logging"]["dir"]; - this->network.mavlink.connect = configs["network"]["mavlink"]["connect"]; - this->network.gcs.port = configs["network"]["gcs"]["port"]; - - this->rrt_config.iterations_per_waypoint = - configs["pathing"]["rrt"]["iterations_per_waypoint"]; - this->rrt_config.rewire_radius = configs["pathing"]["rrt"]["rewire_radius"]; - this->rrt_config.optimize = configs["pathing"]["rrt"]["optimize"]; - this->rrt_config.point_fetch_method = configs["pathing"]["rrt"]["point_fetch_methods"]; - this->rrt_config.allowed_to_skip_waypoints = - configs["pathing"]["rrt"]["allowed_to_skip_waypoints"]; - - this->coverage_pathing_config.coverage_altitude_m = - configs["pathing"]["coverage"]["coverage_altitude_m"]; - this->coverage_pathing_config.optimize = configs["pathing"]["coverage"]["optimize"]; - this->coverage_pathing_config.vertical = configs["pathing"]["coverage"]["vertical"]; - this->coverage_pathing_config.one_way = configs["pathing"]["coverage"]["one_way"]; - - this->cv.matching_model_dir = configs["cv"]["matching_model_dir"]; - this->cv.segmentation_model_dir = configs["cv"]["segmentation_model_dir"]; - this->cv.saliency_model_dir = configs["cv"]["saliency_model_dir"]; - this->airdrop_pathing_config.drop_method = configs["pathing"]["approach"]["drop_method"]; - - this->airdrop_pathing_config.drop_angle_rad = - configs["pathing"]["approach"]["drop_angle_rad"]; - this->airdrop_pathing_config.drop_altitude_m = - configs["pathing"]["approach"]["drop_altitude_m"]; - this->airdrop_pathing_config.guided_drop_distance_m = - configs["pathing"]["approach"]["guided_drop_distance_m"]; - this->airdrop_pathing_config.unguided_drop_distance_m = - configs["pathing"]["approach"]["unguided_drop_distance_m"]; - - this->camera_config.type = configs["camera"]["type"]; - this->camera_config.save_dir = configs["camera"]["save_dir"]; - - this->camera_config.mock.images_dir = configs["camera"]["mock"]["images_dir"]; - - this->camera_config.lucid.sensor_shutter_mode = - configs["camera"]["lucid"]["sensor_shuttle_mode"]; - - this->camera_config.lucid.acquisition_frame_rate_enable = - configs["camera"]["lucid"]["acquisition_frame_rate_enable"]; - this->camera_config.lucid.target_brightness = - configs["camera"]["lucid"]["target_brightness"]; - this->camera_config.lucid.exposure_auto = configs["camera"]["lucid"]["exposure_auto"]; - this->camera_config.lucid.exposure_time = configs["camera"]["lucid"]["exposure_time"]; - this->camera_config.lucid.exposure_auto_damping = - configs["camera"]["lucid"]["exposure_auto_damping"]; - this->camera_config.lucid.exposure_auto_algorithm = - configs["camera"]["lucid"]["exposure_auto_algorithm"]; - this->camera_config.lucid.exposure_auto_upper_limit = - configs["camera"]["lucid"]["exposure_auto_upper_limit"]; - this->camera_config.lucid.exposure_auto_lower_limit = - configs["camera"]["lucid"]["exposure_auto_lower_limit"]; - - this->camera_config.lucid.stream_auto_negotiate_packet_size = - configs["camera"]["lucid"]["stream_auto_negotiate_packet_size"]; - this->camera_config.lucid.stream_packet_resend_enable = - configs["camera"]["lucid"]["stream_packet_resend_enable"]; - - this->camera_config.lucid.device_link_throughput_limit_mode = - configs["camera"]["lucid"]["device_link_throughput_limit_mode"]; - this->camera_config.lucid.device_link_throughput_limit = - configs["camera"]["lucid"]["device_link_throughput_limit"]; - - this->camera_config.lucid.gamma_enable = configs["camera"]["lucid"]["gamma_enable"]; - this->camera_config.lucid.gamma = configs["camera"]["lucid"]["gamma"]; - this->camera_config.lucid.gain_auto = configs["camera"]["lucid"]["gain_auto"]; - this->camera_config.lucid.gain_auto_upper_limit = - configs["camera"]["lucid"]["gain_auto_upper_limit"]; - this->camera_config.lucid.gain_auto_lower_limit = - configs["camera"]["lucid"]["gain_auto_lower_limit"]; - - this->takeoff.altitude_m = configs["takeoff"]["altitude_m"]; - } else { + if (argc != 2) { LOG_F(FATAL, "You must specify a config file. e.g. bin/obcpp ../configs/dev-config.json"); } + + // Load in json file + std::ifstream configStream(argv[1]); + if (!configStream.is_open()) { + throw std::invalid_argument(std::string("Invalid path to config file: ") + + std::string(argv[1])); + } + // the macros expect this to be called "configs" so don't change it without also + // changing the macros + nlohmann::json configs = nlohmann::json::parse(configStream); + + // Set configs + SET_CONFIG_OPT(logging, dir); + SET_CONFIG_OPT(network, mavlink, connect); + SET_CONFIG_OPT(network, gcs, port); + + SET_CONFIG_OPT(pathing, rrt, iterations_per_waypoint); + SET_CONFIG_OPT(pathing, rrt, rewire_radius); + SET_CONFIG_OPT(pathing, rrt, optimize); + + SET_CONFIG_OPT_VARIANT(PointFetchMethod, pathing, rrt, point_fetch_method); + + SET_CONFIG_OPT(pathing, rrt, allowed_to_skip_waypoints); + + 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); + SET_CONFIG_OPT(pathing, coverage, forward, vertical); + SET_CONFIG_OPT(pathing, coverage, forward, one_way); + + SET_CONFIG_OPT(cv, matching_model_dir); + SET_CONFIG_OPT(cv, segmentation_model_dir); + SET_CONFIG_OPT(cv, saliency_model_dir); + + SET_CONFIG_OPT_VARIANT(AirdropDropMethod, pathing, approach, drop_method); + SET_CONFIG_OPT(pathing, approach, drop_angle_rad); + SET_CONFIG_OPT(pathing, approach, drop_altitude_m); + SET_CONFIG_OPT(pathing, approach, guided_drop_distance_m); + SET_CONFIG_OPT(pathing, approach, unguided_drop_distance_m); + + SET_CONFIG_OPT(pathing, dubins, turning_radius); + SET_CONFIG_OPT(pathing, dubins, point_separation); + + SET_CONFIG_OPT(camera, type); + SET_CONFIG_OPT(camera, save_dir); + SET_CONFIG_OPT(camera, mock, images_dir); + + SET_CONFIG_OPT(camera, lucid, sensor_shutter_mode); + SET_CONFIG_OPT(camera, lucid, acquisition_frame_rate_enable); + SET_CONFIG_OPT(camera, lucid, target_brightness); + SET_CONFIG_OPT(camera, lucid, exposure_auto); + SET_CONFIG_OPT(camera, lucid, exposure_time); + SET_CONFIG_OPT(camera, lucid, exposure_auto_damping); + SET_CONFIG_OPT(camera, lucid, exposure_auto_algorithm); + SET_CONFIG_OPT(camera, lucid, exposure_auto_upper_limit); + SET_CONFIG_OPT(camera, lucid, exposure_auto_lower_limit); + + SET_CONFIG_OPT(camera, lucid, stream_auto_negotiate_packet_size); + SET_CONFIG_OPT(camera, lucid, stream_packet_resend_enable); + + SET_CONFIG_OPT(camera, lucid, device_link_throughput_limit_mode); + 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, gain_auto); + SET_CONFIG_OPT(camera, lucid, gain_auto_upper_limit); + SET_CONFIG_OPT(camera, lucid, gain_auto_lower_limit); + + SET_CONFIG_OPT(takeoff, altitude_m); } diff --git a/tests/integration/airdrop_approach.cpp b/tests/integration/airdrop_approach.cpp index bd4f9e2d..e0e09937 100644 --- a/tests/integration/airdrop_approach.cpp +++ b/tests/integration/airdrop_approach.cpp @@ -20,10 +20,15 @@ #include "utilities/datatypes.hpp" #include "utilities/http.hpp" -#define DECLARE_HANDLER_PARAMS(STATE, REQ, RESP) \ - std::shared_ptr STATE = std::make_shared(); \ - httplib::Request REQ; \ - httplib::Response RESP +#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 const static char* mission_json_2024 = R"( { @@ -252,7 +257,7 @@ int main() { AirdropApproachPathing approach(RRTPoint(XYZCoord(-500, 100, 0), 0), XYZCoord(313.131212, -187.781235, 0), RRTPoint(XYZCoord(0, 0, 0), 0), - state->mission_params.getFlightBoundary()); + state->mission_params.getFlightBoundary(), state->config); Environment env(state->mission_params.getFlightBoundary(), state->mission_params.getAirdropBoundary(), {}, {}); diff --git a/tests/integration/camera_lucid.cpp b/tests/integration/camera_lucid.cpp index e50c06c5..a53c9b02 100644 --- a/tests/integration/camera_lucid.cpp +++ b/tests/integration/camera_lucid.cpp @@ -25,7 +25,7 @@ int main (int argc, char *argv[]) { auto mav = std::make_shared("serial:///dev/ttyACM0"); - LucidCamera camera(config.camera_config); + LucidCamera camera(config.camera); camera.connect(); LOG_F(INFO, "Connected to LUCID camera!"); @@ -38,7 +38,7 @@ int main (int argc, char *argv[]) { std::deque images = camera.getAllImages(); for (const ImageData& image : images) { - std::filesystem::path filepath = config.camera_config.save_dir / std::to_string(image.TIMESTAMP); + std::filesystem::path filepath = config.camera.save_dir / std::to_string(image.TIMESTAMP); saveImageToFile(image.DATA, filepath); if (image.TELEMETRY.has_value()) { saveImageTelemetryToFile(image.TELEMETRY.value(), filepath); diff --git a/tests/integration/camera_mock.cpp b/tests/integration/camera_mock.cpp index 9929e617..f8bb05ab 100644 --- a/tests/integration/camera_mock.cpp +++ b/tests/integration/camera_mock.cpp @@ -24,7 +24,7 @@ int main (int argc, char *argv[]) { } OBCConfig config(argc, argv); - MockCamera camera(config.camera_config); + MockCamera camera(config.camera); camera.connect(); @@ -36,7 +36,7 @@ int main (int argc, char *argv[]) { std::deque images = camera.getAllImages(); for (const ImageData& image : images) { - std::filesystem::path save_dir = config.camera_config.save_dir; + std::filesystem::path save_dir = config.camera.save_dir; std::filesystem::path img_filepath = save_dir / (std::to_string(image.TIMESTAMP) + std::string(".jpg")); std::filesystem::path json_filepath = save_dir / (std::to_string(image.TIMESTAMP) + std::string(".json")); saveImageToFile(image.DATA, img_filepath); diff --git a/tests/integration/coverage_pathing.cpp b/tests/integration/coverage_pathing.cpp index f2610401..9a6bd94a 100644 --- a/tests/integration/coverage_pathing.cpp +++ b/tests/integration/coverage_pathing.cpp @@ -12,20 +12,26 @@ #include "network/gcs_routes.hpp" #include "pathing/plotting.hpp" #include "pathing/static.hpp" -#include "ticks/mission_prep.hpp" #include "ticks/mav_upload.hpp" +#include "ticks/mission_prep.hpp" #include "ticks/path_gen.hpp" #include "ticks/tick.hpp" #include "utilities/constants.hpp" #include "utilities/datatypes.hpp" #include "utilities/http.hpp" -#define DECLARE_HANDLER_PARAMS(STATE, REQ, RESP) \ - std::shared_ptr STATE = std::make_shared(); \ - httplib::Request REQ; \ +#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 -const static char* mission_json_2020 = R"( +const static char *mission_json_2020 = R"( { "BottleAssignments": [ { @@ -257,10 +263,12 @@ int main() { RRTPoint start = RRTPoint(state->mission_params.getWaypoints()[0], 0); - CoveragePathing search(start, 20, state->mission_params.getFlightBoundary(), - state->mission_params.getAirdropBoundary(), {}, {true, true, true}); + ForwardCoveragePathing search(start, 20, state->mission_params.getFlightBoundary(), + state->mission_params.getAirdropBoundary(), state->config, {}); + std::cout << "Running Search" << std::endl; std::vector path = search.run(); + std::cout << "Search Complete" << std::endl; // plot the path std::cout << "Start Plotting" << std::endl; diff --git a/tests/integration/coverage_pathing_2.cpp b/tests/integration/coverage_pathing_2.cpp index 51e818b4..8a859895 100644 --- a/tests/integration/coverage_pathing_2.cpp +++ b/tests/integration/coverage_pathing_2.cpp @@ -20,9 +20,15 @@ #include "utilities/datatypes.hpp" #include "utilities/http.hpp" -#define DECLARE_HANDLER_PARAMS(STATE, REQ, RESP) \ - std::shared_ptr STATE = std::make_shared(); \ - httplib::Request REQ; \ +#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 const static char* mission_json_2024 = R"( @@ -251,10 +257,8 @@ int main() { RRTPoint start = RRTPoint(state->mission_params.getWaypoints()[0], 0); start.coord.z = 100; - CoveragePathing search( - start, 9, state->mission_params.getFlightBoundary(), - state->mission_params.getAirdropBoundary(), {}, - AirdropSearchConfig{.coverage_altitude_m = 30.0, .optimize = false, .vertical = false, .one_way = false}); + ForwardCoveragePathing search(start, 9, state->mission_params.getFlightBoundary(), + state->mission_params.getAirdropBoundary(), state->config, {}); Environment env(state->mission_params.getFlightBoundary(), state->mission_params.getAirdropBoundary(), {}, {}); @@ -264,7 +268,7 @@ int main() { std::vector path = search.run(); for (const XYZCoord& coord : path) { - file << coord.z << std::endl; + file << coord.x << " " << coord.y << " " << coord.z << std::endl; } // plot the path diff --git a/tests/integration/path_planning.cpp b/tests/integration/path_planning.cpp index 7a104653..08df97d5 100644 --- a/tests/integration/path_planning.cpp +++ b/tests/integration/path_planning.cpp @@ -20,10 +20,15 @@ #include "utilities/datatypes.hpp" #include "utilities/http.hpp" -#define DECLARE_HANDLER_PARAMS(STATE, REQ, RESP) \ - std::shared_ptr STATE = std::make_shared(); \ - httplib::Request REQ; \ - httplib::Response RESP +#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 const static char* mission_json_2020 = R"( { @@ -277,18 +282,12 @@ int main() { start.coord.z = 30.0; // 30 meters takeoff // RRT settings (manually put in) - int num_iterations = 512; double search_radius = 9999; - double rewire_radius = 256; - RRTConfig config = RRTConfig { num_iterations, rewire_radius, true, POINT_FETCH_METHODS::NEAREST, true }; - RRT rrt = RRT(start, goals, search_radius, - state->mission_params.getFlightBoundary(), obstacles, {}, config); + RRT rrt = RRT(start, goals, search_radius, + state->mission_params.getFlightBoundary(), state->config, obstacles, {}); - // print out stats - std::cout << "num_iterations: " << num_iterations << std::endl; std::cout << "search_radius: " << search_radius << std::endl; - std::cout << "rewire_radius: " << rewire_radius << std::endl; //run the algoritm, and time it auto start_time = std::chrono::high_resolution_clock::now(); diff --git a/tests/integration/pathing_integration.cpp b/tests/integration/pathing_integration.cpp index 6b7ca5a0..232af354 100644 --- a/tests/integration/pathing_integration.cpp +++ b/tests/integration/pathing_integration.cpp @@ -249,7 +249,7 @@ int main() { DECLARE_HANDLER_PARAMS(state, req, resp); req.body = mission_json_2020; state->setTick(new MissionPrepTick(state)); - std::cout << state->config.rrt_config.iterations_per_waypoint << std::endl; + std::cout << state->config.rrt.iterations_per_waypoint << std::endl; GCS_HANDLE(Post, mission)(state, req, resp);