From 2148e9279b4e92023e58ee575368231fade6e76c Mon Sep 17 00:00:00 2001 From: Tyler Lentz Date: Mon, 10 Jun 2024 20:05:55 +0000 Subject: [PATCH 01/13] refactor config --- include/camera/interface.hpp | 1 + include/pathing/static.hpp | 16 +++ include/utilities/datatypes.hpp | 67 ------------ include/utilities/obc_config.hpp | 122 ++++++++++++++++++---- src/core/obc.cpp | 10 +- src/network/gcs_routes.cpp | 2 +- src/pathing/static.cpp | 2 +- src/utilities/obc_config.cpp | 72 +++++++------ tests/integration/camera_lucid.cpp | 4 +- tests/integration/camera_mock.cpp | 4 +- tests/integration/pathing_integration.cpp | 2 +- 11 files changed, 163 insertions(+), 139 deletions(-) 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..27b3e6aa 100644 --- a/include/pathing/static.hpp +++ b/include/pathing/static.hpp @@ -190,6 +190,10 @@ 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 { public: @@ -239,6 +243,18 @@ class CoveragePathing { const AirdropSearchConfig 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. + */ +class HoverCoveragePathing { + public: + HoverCoveragePathing(const RRTPoint& start, const XYZCoord& goal, Polygon flight_bounds, + Polygon drop_zone); + private: + +}; + class AirdropApproachPathing { public: AirdropApproachPathing(const RRTPoint &start, const XYZCoord &goal, RRTPoint wind, diff --git a/include/utilities/datatypes.hpp b/include/utilities/datatypes.hpp index e0b2b8bc..cc2a822c 100644 --- a/include/utilities/datatypes.hpp +++ b/include/utilities/datatypes.hpp @@ -99,72 +99,5 @@ enum POINT_FETCH_METHODS { // 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..fc0fb8f1 100644 --- a/include/utilities/obc_config.hpp +++ b/include/utilities/obc_config.hpp @@ -6,34 +6,110 @@ #include "utilities/constants.hpp" #include "utilities/datatypes.hpp" -struct OBCConfig { - struct { - std::string dir; - } logging; +struct LoggingConfig { + std::string dir; +}; +struct NetworkConfig { struct { - struct { - int port; - } gcs; - struct { - std::string connect; - } mavlink; - } network; - + int port; + } gcs; struct { - float altitude_m; - } takeoff; + 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; +}; + +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 PathingConfig { + RRTConfig rrt; + AirdropSearchConfig coverage; + AirdropApproachConfig approach; +}; + +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 { - 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; + // 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; + PathingConfig pathing; + CameraConfig camera; // Load user specified config json, or make a new one OBCConfig(int argc, char* argv[]); 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/static.cpp b/src/pathing/static.cpp index fe66a502..f590e12b 100644 --- a/src/pathing/static.cpp +++ b/src/pathing/static.cpp @@ -461,7 +461,7 @@ std::vector generateInitialPath(std::shared_ptr state) { start.coord.z = state->config.takeoff.altitude_m; RRT rrt(start, goals, SEARCH_RADIUS, state->mission_params.getFlightBoundary(), {}, {}, - state->config.rrt_config); + state->config.pathing.rrt); rrt.run(); diff --git a/src/utilities/obc_config.cpp b/src/utilities/obc_config.cpp index cb446ad0..5a11cf4b 100644 --- a/src/utilities/obc_config.cpp +++ b/src/utilities/obc_config.cpp @@ -29,73 +29,71 @@ OBCConfig::OBCConfig(int argc, char* argv[]) { this->network.mavlink.connect = configs["network"]["mavlink"]["connect"]; this->network.gcs.port = configs["network"]["gcs"]["port"]; - this->rrt_config.iterations_per_waypoint = + this->pathing.rrt.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 = + this->pathing.rrt.rewire_radius = configs["pathing"]["rrt"]["rewire_radius"]; + this->pathing.rrt.optimize = configs["pathing"]["rrt"]["optimize"]; + this->pathing.rrt.point_fetch_method = configs["pathing"]["rrt"]["point_fetch_methods"]; + this->pathing.rrt.allowed_to_skip_waypoints = configs["pathing"]["rrt"]["allowed_to_skip_waypoints"]; - this->coverage_pathing_config.coverage_altitude_m = + this->pathing.coverage.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->pathing.coverage.optimize = configs["pathing"]["coverage"]["optimize"]; + this->pathing.coverage.vertical = configs["pathing"]["coverage"]["vertical"]; + this->pathing.coverage.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 = + this->pathing.approach.drop_method = configs["pathing"]["approach"]["drop_method"]; + this->pathing.approach.drop_angle_rad = configs["pathing"]["approach"]["drop_angle_rad"]; - this->airdrop_pathing_config.drop_altitude_m = + this->pathing.approach.drop_altitude_m = configs["pathing"]["approach"]["drop_altitude_m"]; - this->airdrop_pathing_config.guided_drop_distance_m = + this->pathing.approach.guided_drop_distance_m = configs["pathing"]["approach"]["guided_drop_distance_m"]; - this->airdrop_pathing_config.unguided_drop_distance_m = + this->pathing.approach.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.type = configs["camera"]["type"]; + this->camera.save_dir = configs["camera"]["save_dir"]; + this->camera.mock.images_dir = configs["camera"]["mock"]["images_dir"]; - this->camera_config.mock.images_dir = configs["camera"]["mock"]["images_dir"]; - - this->camera_config.lucid.sensor_shutter_mode = + this->camera.lucid.sensor_shutter_mode = configs["camera"]["lucid"]["sensor_shuttle_mode"]; - - this->camera_config.lucid.acquisition_frame_rate_enable = + this->camera.lucid.acquisition_frame_rate_enable = configs["camera"]["lucid"]["acquisition_frame_rate_enable"]; - this->camera_config.lucid.target_brightness = + this->camera.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 = + this->camera.lucid.exposure_auto = configs["camera"]["lucid"]["exposure_auto"]; + this->camera.lucid.exposure_time = configs["camera"]["lucid"]["exposure_time"]; + this->camera.lucid.exposure_auto_damping = configs["camera"]["lucid"]["exposure_auto_damping"]; - this->camera_config.lucid.exposure_auto_algorithm = + this->camera.lucid.exposure_auto_algorithm = configs["camera"]["lucid"]["exposure_auto_algorithm"]; - this->camera_config.lucid.exposure_auto_upper_limit = + this->camera.lucid.exposure_auto_upper_limit = configs["camera"]["lucid"]["exposure_auto_upper_limit"]; - this->camera_config.lucid.exposure_auto_lower_limit = + this->camera.lucid.exposure_auto_lower_limit = configs["camera"]["lucid"]["exposure_auto_lower_limit"]; - this->camera_config.lucid.stream_auto_negotiate_packet_size = + this->camera.lucid.stream_auto_negotiate_packet_size = configs["camera"]["lucid"]["stream_auto_negotiate_packet_size"]; - this->camera_config.lucid.stream_packet_resend_enable = + this->camera.lucid.stream_packet_resend_enable = configs["camera"]["lucid"]["stream_packet_resend_enable"]; - this->camera_config.lucid.device_link_throughput_limit_mode = + this->camera.lucid.device_link_throughput_limit_mode = configs["camera"]["lucid"]["device_link_throughput_limit_mode"]; - this->camera_config.lucid.device_link_throughput_limit = + this->camera.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 = + this->camera.lucid.gamma_enable = configs["camera"]["lucid"]["gamma_enable"]; + this->camera.lucid.gamma = configs["camera"]["lucid"]["gamma"]; + this->camera.lucid.gain_auto = configs["camera"]["lucid"]["gain_auto"]; + this->camera.lucid.gain_auto_upper_limit = configs["camera"]["lucid"]["gain_auto_upper_limit"]; - this->camera_config.lucid.gain_auto_lower_limit = + this->camera.lucid.gain_auto_lower_limit = configs["camera"]["lucid"]["gain_auto_lower_limit"]; this->takeoff.altitude_m = configs["takeoff"]["altitude_m"]; 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/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); From 977243de246240dfc8bbf2d9613520ea6cfd4e01 Mon Sep 17 00:00:00 2001 From: Tyler Lentz Date: Mon, 10 Jun 2024 22:11:14 +0000 Subject: [PATCH 02/13] beautiful, beautiful macros --- configs/config.json | 25 ++-- configs/dev-config.json | 25 ++-- include/pathing/static.hpp | 36 ++++-- include/pathing/tree.hpp | 2 +- include/utilities/datatypes.hpp | 6 - include/utilities/obc_config.hpp | 59 +++++++-- include/utilities/obc_config_macros.hpp | 104 ++++++++++++++++ src/pathing/static.cpp | 39 +++--- src/pathing/tree.cpp | 9 +- src/utilities/obc_config.cpp | 157 +++++++++++------------- tests/integration/path_planning.cpp | 2 +- 11 files changed, 311 insertions(+), 153 deletions(-) create mode 100644 include/utilities/obc_config_macros.hpp diff --git a/configs/config.json b/configs/config.json index 70848352..9c40cd03 100644 --- a/configs/config.json +++ b/configs/config.json @@ -14,23 +14,28 @@ "altitude_m": 30.0 }, "pathing": { - "_comment": "Point Fetch Method 2: NEAREST", - "_comment2": "Drop Method 1: UNGUIDED", "rrt": { "iterations_per_waypoint": 200, "rewire_radius": 200.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, + "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 +55,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..b8b45c04 100644 --- a/configs/dev-config.json +++ b/configs/dev-config.json @@ -14,23 +14,28 @@ "altitude_m": 30.0 }, "pathing": { - "_comment": "Point Fetch Method 2: NEAREST", - "_comment2": "Drop Method 1: UNGUIDED", "rrt": { "iterations_per_waypoint": 200, "rewire_radius": 200.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, + "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 +55,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/pathing/static.hpp b/include/pathing/static.hpp index 27b3e6aa..6962a85b 100644 --- a/include/pathing/static.hpp +++ b/include/pathing/static.hpp @@ -27,14 +27,14 @@ class RRT { RRTConfig config = {.iterations_per_waypoint = ITERATIONS_PER_WAYPOINT, .rewire_radius = REWIRE_RADIUS, .optimize = false, - .point_fetch_method = POINT_FETCH_METHODS::NEAREST, + .point_fetch_method = PointFetchMethod::Enum::NEAREST, .allowed_to_skip_waypoints = false}); 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, + .point_fetch_method = PointFetchMethod::Enum::NONE, .allowed_to_skip_waypoints = false}); /** @@ -195,14 +195,16 @@ class RRT { * - 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, + ForwardCoveragePathing(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}); + 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 @@ -240,19 +242,25 @@ 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(const RRTPoint& start, const XYZCoord& goal, Polygon flight_bounds, - Polygon drop_zone); - private: + HoverCoveragePathing(Polygon flight_bounds, Polygon drop_zone); + + std::vector run(); + private: + Polygon flight_bounds; + Polygon drop_zone; }; class AirdropApproachPathing { @@ -260,7 +268,7 @@ class AirdropApproachPathing { AirdropApproachPathing(const RRTPoint &start, const XYZCoord &goal, RRTPoint wind, Polygon bounds, std::vector obstacles = {}, AirdropApproachConfig config = { - .drop_method = UNGUIDED, + .drop_method = AirdropDropMethod::Enum::UNGUIDED, .bottle_ids = {1, 2, 3, 4, 5}, .drop_angle_rad = DROP_ANGLE_RAD, // .drop_angle_rad = M_PI * 3 / 4, @@ -291,4 +299,6 @@ class AirdropApproachPathing { 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..ff20009d 100644 --- a/include/pathing/tree.hpp +++ b/include/pathing/tree.hpp @@ -196,7 +196,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/datatypes.hpp b/include/utilities/datatypes.hpp index cc2a822c..8cf961e3 100644 --- a/include/utilities/datatypes.hpp +++ b/include/utilities/datatypes.hpp @@ -92,12 +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) -}; #endif // INCLUDE_UTILITIES_DATATYPES_HPP_ diff --git a/include/utilities/obc_config.hpp b/include/utilities/obc_config.hpp index fc0fb8f1..90c7ae7e 100644 --- a/include/utilities/obc_config.hpp +++ b/include/utilities/obc_config.hpp @@ -1,11 +1,15 @@ #ifndef INCLUDE_UTILITIES_OBC_CONFIG_HPP_ #define INCLUDE_UTILITIES_OBC_CONFIG_HPP_ +#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> + struct LoggingConfig { std::string dir; }; @@ -29,24 +33,63 @@ struct CVConfig { 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} + }; +}; + 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; + PointFetchMethod::Enum 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 +namespace AirdropCoverageMethod { + enum class Enum { + HOVER, + FORWARD + }; + CONFIG_VARIANT_MAPPING_T(Enum) MAPPINGS = { + {"hover", Enum::HOVER}, {"forward", Enum::FORWARD} + }; +}; + +struct AirdropCoverageConfig { + double altitude_m; + AirdropCoverageMethod::Enum method; + struct { + 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 { - drop_mode drop_method; + AirdropDropMethod::Enum drop_method; std::unordered_set bottle_ids; double drop_angle_rad; double drop_altitude_m; @@ -56,7 +99,7 @@ struct AirdropApproachConfig { struct PathingConfig { RRTConfig rrt; - AirdropSearchConfig coverage; + AirdropCoverageConfig coverage; AirdropApproachConfig approach; }; diff --git a/include/utilities/obc_config_macros.hpp b/include/utilities/obc_config_macros.hpp new file mode 100644 index 00000000..61bba4aa --- /dev/null +++ b/include/utilities/obc_config_macros.hpp @@ -0,0 +1,104 @@ +#ifndef INCLUDE_UTILITIES_OBC_CONFIG_MACROS_HPP_ +#define INCLUDE_UTILITIES_OBC_CONFIG_MACROS_HPP_ + +/* + ( ) /\ _ ( + \ | ( \ ( \.( ) _____ + \ \ \ ` ` ) \ ( ___ / _ \ + (_` \+ . 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_ \ No newline at end of file diff --git a/src/pathing/static.cpp b/src/pathing/static.cpp index f590e12b..b28a612b 100644 --- a/src/pathing/static.cpp +++ b/src/pathing/static.cpp @@ -15,6 +15,7 @@ #include "pathing/plotting.hpp" #include "pathing/tree.hpp" #include "utilities/constants.hpp" +#include "utilities/obc_config.hpp" #include "utilities/datatypes.hpp" #include "utilities/rng.hpp" @@ -177,7 +178,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 +281,23 @@ 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, +ForwardCoveragePathing::ForwardCoveragePathing(const RRTPoint &start, double scan_radius, Polygon bounds, Polygon airdrop_zone, std::vector obstacles, - AirdropSearchConfig config) + AirdropCoverageConfig config) : start(start), scan_radius(scan_radius), airspace(Environment(bounds, airdrop_zone, {}, obstacles)), dubins(Dubins(TURNING_RADIUS, POINT_SEPARATION)), config(config) {} -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 +309,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 +362,13 @@ std::vector CoveragePathing::coverageOptimal() const { return generatePath(dubins_paths[best_path_idx], waypoints); } -std::vector CoveragePathing::generatePath(const std::vector &dubins_options, +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 +389,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()); @@ -418,11 +419,15 @@ std::vector AirdropApproachPathing::run() const { 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_distance = 0.0f; + if (config.drop_method == AirdropDropMethod::Enum::GUIDED) { + drop_distance = config.guided_drop_distance_m; + } else { + drop_distance = config.unguided_drop_distance_m; + } - XYZCoord drop_offset(drop_distance * std::cos(drop_angle), drop_distance * std::sin(drop_angle), - 0); + XYZCoord drop_offset( + drop_distance * std::cos(drop_angle), drop_distance * std::sin(drop_angle), 0); double wind_strength_coef = wind.coord.norm() * WIND_CONST_PER_ALTITUDE; XYZCoord wind_offset(wind_strength_coef * std::cos(wind.psi), @@ -475,3 +480,7 @@ std::vector generateInitialPath(std::shared_ptr state) { return output_coords; } + +std::vector generateSearchPath(std::shared_ptr state) { + return {}; +} \ No newline at end of file diff --git a/src/pathing/tree.cpp b/src/pathing/tree.cpp index 4c8392a0..de4ad710 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) @@ -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 5a11cf4b..c8ac5687 100644 --- a/src/utilities/obc_config.cpp +++ b/src/utilities/obc_config.cpp @@ -10,94 +10,81 @@ #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->pathing.rrt.iterations_per_waypoint = - configs["pathing"]["rrt"]["iterations_per_waypoint"]; - this->pathing.rrt.rewire_radius = configs["pathing"]["rrt"]["rewire_radius"]; - this->pathing.rrt.optimize = configs["pathing"]["rrt"]["optimize"]; - this->pathing.rrt.point_fetch_method = configs["pathing"]["rrt"]["point_fetch_methods"]; - this->pathing.rrt.allowed_to_skip_waypoints = - configs["pathing"]["rrt"]["allowed_to_skip_waypoints"]; - - this->pathing.coverage.coverage_altitude_m = - configs["pathing"]["coverage"]["coverage_altitude_m"]; - this->pathing.coverage.optimize = configs["pathing"]["coverage"]["optimize"]; - this->pathing.coverage.vertical = configs["pathing"]["coverage"]["vertical"]; - this->pathing.coverage.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->pathing.approach.drop_method = configs["pathing"]["approach"]["drop_method"]; - this->pathing.approach.drop_angle_rad = - configs["pathing"]["approach"]["drop_angle_rad"]; - this->pathing.approach.drop_altitude_m = - configs["pathing"]["approach"]["drop_altitude_m"]; - this->pathing.approach.guided_drop_distance_m = - configs["pathing"]["approach"]["guided_drop_distance_m"]; - this->pathing.approach.unguided_drop_distance_m = - configs["pathing"]["approach"]["unguided_drop_distance_m"]; - - this->camera.type = configs["camera"]["type"]; - this->camera.save_dir = configs["camera"]["save_dir"]; - this->camera.mock.images_dir = configs["camera"]["mock"]["images_dir"]; - - this->camera.lucid.sensor_shutter_mode = - configs["camera"]["lucid"]["sensor_shuttle_mode"]; - this->camera.lucid.acquisition_frame_rate_enable = - configs["camera"]["lucid"]["acquisition_frame_rate_enable"]; - this->camera.lucid.target_brightness = - configs["camera"]["lucid"]["target_brightness"]; - this->camera.lucid.exposure_auto = configs["camera"]["lucid"]["exposure_auto"]; - this->camera.lucid.exposure_time = configs["camera"]["lucid"]["exposure_time"]; - this->camera.lucid.exposure_auto_damping = - configs["camera"]["lucid"]["exposure_auto_damping"]; - this->camera.lucid.exposure_auto_algorithm = - configs["camera"]["lucid"]["exposure_auto_algorithm"]; - this->camera.lucid.exposure_auto_upper_limit = - configs["camera"]["lucid"]["exposure_auto_upper_limit"]; - this->camera.lucid.exposure_auto_lower_limit = - configs["camera"]["lucid"]["exposure_auto_lower_limit"]; - - this->camera.lucid.stream_auto_negotiate_packet_size = - configs["camera"]["lucid"]["stream_auto_negotiate_packet_size"]; - this->camera.lucid.stream_packet_resend_enable = - configs["camera"]["lucid"]["stream_packet_resend_enable"]; - - this->camera.lucid.device_link_throughput_limit_mode = - configs["camera"]["lucid"]["device_link_throughput_limit_mode"]; - this->camera.lucid.device_link_throughput_limit = - configs["camera"]["lucid"]["device_link_throughput_limit"]; - - this->camera.lucid.gamma_enable = configs["camera"]["lucid"]["gamma_enable"]; - this->camera.lucid.gamma = configs["camera"]["lucid"]["gamma"]; - this->camera.lucid.gain_auto = configs["camera"]["lucid"]["gain_auto"]; - this->camera.lucid.gain_auto_upper_limit = - configs["camera"]["lucid"]["gain_auto_upper_limit"]; - this->camera.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, 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(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/path_planning.cpp b/tests/integration/path_planning.cpp index 7a104653..6bae26e5 100644 --- a/tests/integration/path_planning.cpp +++ b/tests/integration/path_planning.cpp @@ -280,7 +280,7 @@ int main() { 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 }; + RRTConfig config = RRTConfig { num_iterations, rewire_radius, true, PointFetchMethod::Enum::NEAREST, true }; RRT rrt = RRT(start, goals, search_radius, state->mission_params.getFlightBoundary(), obstacles, {}, config); From 94cf080d3042a9b7c9619f0c4ef1cbf578bb591c Mon Sep 17 00:00:00 2001 From: Tyler Lentz Date: Mon, 10 Jun 2024 22:45:24 +0000 Subject: [PATCH 03/13] fix lint & add camera vision config option --- configs/config.json | 1 + configs/dev-config.json | 1 + include/pathing/static.hpp | 19 ++++++----- include/utilities/obc_config.hpp | 12 ++++--- include/utilities/obc_config_macros.hpp | 18 +++++----- src/pathing/static.cpp | 45 ++++++++++++++++++------- src/utilities/obc_config.cpp | 3 +- 7 files changed, 65 insertions(+), 34 deletions(-) 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); From 7d54a5e266d1d1ec7c49cac6baa502e056c76a2e Mon Sep 17 00:00:00 2001 From: Tyler Lentz Date: Mon, 10 Jun 2024 23:01:29 +0000 Subject: [PATCH 04/13] log read in config options --- include/utilities/obc_config_macros.hpp | 28 ++++++++++++++++++------- src/main.cpp | 3 --- src/utilities/obc_config.cpp | 7 ++++++- 3 files changed, 26 insertions(+), 12 deletions(-) diff --git a/include/utilities/obc_config_macros.hpp b/include/utilities/obc_config_macros.hpp index aa8a5bf7..de085ec8 100644 --- a/include/utilities/obc_config_macros.hpp +++ b/include/utilities/obc_config_macros.hpp @@ -78,14 +78,26 @@ constexpr T PARSE_VARIANT(CONFIG_VARIANT_MAPPING_T(T) mapping, std::string input // 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_5(p1, p2, p3, p4, p5) { \ + auto val = PARSE(p1, p2, p3, p4, p5); \ + LOG_F(INFO, "%s.%s.%s.%s.%s = %s", #p1, #p2, #p3, #p4, #p5, val.dump().c_str()); \ + this->p1.p2.p3.p4.p5 = val; \ +} +#define SET_CONFIG_OPT_4(p1, p2, p3, p4) { \ + auto val = PARSE(p1, p2, p3, p4); \ + LOG_F(INFO, "%s.%s.%s.%s = %s", #p1, #p2, #p3, #p4, val.dump().c_str()); \ + this->p1.p2.p3.p4 = val; \ +} +#define SET_CONFIG_OPT_3(p1, p2, p3) { \ + auto val = PARSE(p1, p2, p3); \ + LOG_F(INFO, "%s.%s.%s = %s", #p1, #p2, #p3, val.dump().c_str()); \ + this->p1.p2.p3 = val; \ +} +#define SET_CONFIG_OPT_2(p1, p2) { \ + auto val = PARSE(p1, p2); \ + LOG_F(INFO, "%s.%s = %s", #p1, #p2, val.dump().c_str()); \ + this->p1.p2 = val; \ +} #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__) diff --git a/src/main.cpp b/src/main.cpp index e2351840..921679d9 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -13,9 +13,6 @@ extern "C" { int main(int argc, char* argv[]) { OBCConfig config(argc, argv); - // TODO: pull logging folder from config - initLogging(config.logging.dir, true, argc, argv); - OBC obc(config); obc.run(); } diff --git a/src/utilities/obc_config.cpp b/src/utilities/obc_config.cpp index fff3c2db..4be6777b 100644 --- a/src/utilities/obc_config.cpp +++ b/src/utilities/obc_config.cpp @@ -28,8 +28,13 @@ OBCConfig::OBCConfig(int argc, char* argv[]) { // changing the macros nlohmann::json configs = nlohmann::json::parse(configStream); - // Set configs + + // Read this in first before anything else so that all of the read in values get logged + // to the config file. Otherwise they will be output to the terminal but not saved to + // the file. SET_CONFIG_OPT(logging, dir); + initLogging(this->logging.dir, true, argc, argv); + SET_CONFIG_OPT(network, mavlink, connect); SET_CONFIG_OPT(network, gcs, port); From 552d40c8169677fc0f423c6a773afe6fdb35758d Mon Sep 17 00:00:00 2001 From: Tyler Lentz Date: Mon, 10 Jun 2024 23:42:12 +0000 Subject: [PATCH 05/13] generate hover search path --- include/pathing/static.hpp | 7 ++-- include/pathing/tree.hpp | 1 + include/ticks/path_gen.hpp | 3 +- src/pathing/static.cpp | 68 +++++++++++++++++++++++++++++++++--- src/ticks/path_gen.cpp | 14 +++++--- src/utilities/obc_config.cpp | 1 - 6 files changed, 82 insertions(+), 12 deletions(-) diff --git a/include/pathing/static.hpp b/include/pathing/static.hpp index 9d14160a..25f006ec 100644 --- a/include/pathing/static.hpp +++ b/include/pathing/static.hpp @@ -254,15 +254,18 @@ class ForwardCoveragePathing { * * This outputs a series of XYZ Coordinates which represent a points at which the plane * should hover and take a picture. + * + * Assumptions: + * - The drop zone has 4 points which form a rectangle larger than the vision of the camera */ class HoverCoveragePathing { public: - HoverCoveragePathing(Polygon flight_bounds, Polygon drop_zone); + HoverCoveragePathing(Polygon drop_zone, AirdropCoverageConfig config); std::vector run(); private: - Polygon flight_bounds; + AirdropCoverageConfig config; Polygon drop_zone; }; diff --git a/include/pathing/tree.hpp b/include/pathing/tree.hpp index ff20009d..316e8d90 100644 --- a/include/pathing/tree.hpp +++ b/include/pathing/tree.hpp @@ -11,6 +11,7 @@ #include "pathing/environment.hpp" #include "utilities/datatypes.hpp" #include "utilities/rng.hpp" +#include "utilities/obc_config.hpp" class RRTNode; typedef std::vector RRTNodeList; diff --git a/include/ticks/path_gen.hpp b/include/ticks/path_gen.hpp index f22dd948..ee6cadf9 100644 --- a/include/ticks/path_gen.hpp +++ b/include/ticks/path_gen.hpp @@ -25,7 +25,8 @@ class PathGenTick : public Tick { Tick* tick() override; private: - std::future> path; + std::future> init_path; + std::future> search_path; void startPathGeneration(); }; diff --git a/src/pathing/static.cpp b/src/pathing/static.cpp index 5bff2c8e..c3fa633c 100644 --- a/src/pathing/static.cpp +++ b/src/pathing/static.cpp @@ -398,12 +398,72 @@ std::vector ForwardCoveragePathing::generatePath( return path; } -HoverCoveragePathing::HoverCoveragePathing(Polygon flight_bounds, Polygon drop_zone): - flight_bounds{flight_bounds}, drop_zone{drop_zone} +HoverCoveragePathing::HoverCoveragePathing(Polygon drop_zone, AirdropCoverageConfig config): + config{config}, drop_zone{drop_zone} {} std::vector HoverCoveragePathing::run() { - return {}; + if (this->drop_zone.size() != 4) { + // right now just hardcoded to rectangles. think its ok to panic here because + // we would want to stop early if we messed this up and this will happen + // before takeoff + LOG_F(FATAL, "Hover airdrop pathing currently only supports 4 coordinates, not %lu", this->drop_zone.size()); + } + + XYZCoord top_left(std::numeric_limits::max(), std::numeric_limits::min(), 0); + XYZCoord top_right(std::numeric_limits::min(), std::numeric_limits::min(), 0); + XYZCoord bottom_left(std::numeric_limits::max(), std::numeric_limits::max(), 0); + XYZCoord bottom_right(std::numeric_limits::min(), std::numeric_limits::max(), 0); + + // thx chat gpt + for (const auto& coord : this->drop_zone) { + // Update top-left + if (coord.y > top_left.y || (coord.y == top_left.y && coord.x < top_left.x)) { + top_left = coord; + } + + // Update bottom-left + if (coord.y < bottom_left.y || (coord.y == bottom_left.y && coord.x < bottom_left.x)) { + bottom_left = coord; + } + + // Update top-right + if (coord.y > top_right.y || (coord.y == top_right.y && coord.x > top_right.x)) { + top_right = coord; + } + + // Update bottom-right + if (coord.y < bottom_right.y || (coord.y == bottom_right.y && coord.x > bottom_right.x)) { + bottom_right = coord; + } + } + + // assuming a rectangle!! + + std::vector hover_points; + + double vision = this->config.camera_vision_m; + double altitude = this->config.altitude_m; + + double start_y = top_left.y - (vision / 2.0); + double stop_y = bottom_left.y - (vision / 2.0); + double start_x = top_left.x + (vision / 2.0); + double stop_x = top_right.x + (vision / 2.0); + + bool right = true; // start going from right to left + for (double y = start_y; y < stop_y; y += vision) { + std::vector row; // row of points either from left to right or right to left + for (double x = start_x; x < stop_x; x += vision) { + row.push_back(XYZCoord(x, y, altitude)); + } + if (!right) { + std::reverse(row.begin(), row.end()); + } + right = !right; + hover_points.insert(std::end(hover_points), std::begin(row), std::end(row)); + } + + return hover_points; } AirdropApproachPathing::AirdropApproachPathing(const RRTPoint &start, const XYZCoord &goal, @@ -495,7 +555,7 @@ std::vector generateSearchPath(std::shared_ptr state) { return {}; } else { // hover const auto& [flight_bounds, airdrop_bounds, _, __] = state->mission_params.getConfig(); - HoverCoveragePathing pathing(flight_bounds, airdrop_bounds); + HoverCoveragePathing pathing(airdrop_bounds, state->config.pathing.coverage); std::vector coords; for (const XYZCoord& coord : pathing.run()) { coords.push_back(state->getCartesianConverter()->toLatLng(coord)); diff --git a/src/ticks/path_gen.cpp b/src/ticks/path_gen.cpp index 4e2b0afb..255dd377 100644 --- a/src/ticks/path_gen.cpp +++ b/src/ticks/path_gen.cpp @@ -11,21 +11,27 @@ #include "ticks/path_validate.hpp" #include "utilities/logging.hpp" +using namespace std::chrono_literals; + PathGenTick::PathGenTick(std::shared_ptr state) : Tick(state, TickID::PathGen) { startPathGeneration(); } void PathGenTick::startPathGeneration() { - this->path = std::async(std::launch::async, generateInitialPath, this->state); + this->init_path = std::async(std::launch::async, generateInitialPath, this->state); + this->search_path = std::async(std::launch::async, generateSearchPath, this->state); } std::chrono::milliseconds PathGenTick::getWait() const { return PATH_GEN_TICK_WAIT; } Tick* PathGenTick::tick() { - auto status = this->path.wait_for(std::chrono::milliseconds(0)); - if (status == std::future_status::ready) { + auto init_status = this->init_path.wait_for(0ms); + auto search_status = this->search_path.wait_for(0ms); + if (init_status == std::future_status::ready && + search_status == std::future_status::ready) { LOG_F(INFO, "Initial path generated"); - state->setInitPath(path.get()); + state->setInitPath(this->init_path.get()); + state->setSearchPath(this->search_path.get()); return new PathValidateTick(this->state); } diff --git a/src/utilities/obc_config.cpp b/src/utilities/obc_config.cpp index 4be6777b..8b503f78 100644 --- a/src/utilities/obc_config.cpp +++ b/src/utilities/obc_config.cpp @@ -28,7 +28,6 @@ OBCConfig::OBCConfig(int argc, char* argv[]) { // changing the macros nlohmann::json configs = nlohmann::json::parse(configStream); - // Read this in first before anything else so that all of the read in values get logged // to the config file. Otherwise they will be output to the terminal but not saved to // the file. From b1a9fd0251cede67ab8b8939f02df5fc1cba26db Mon Sep 17 00:00:00 2001 From: Tyler Lentz Date: Tue, 11 Jun 2024 01:02:06 +0000 Subject: [PATCH 06/13] kind of working coverage path --- include/core/mission_state.hpp | 6 +++--- include/network/gcs_routes.hpp | 17 +++++++++-------- include/ticks/path_gen.hpp | 2 +- src/core/mission_state.cpp | 8 ++++---- src/network/gcs.cpp | 1 + src/network/gcs_routes.cpp | 19 +++++++++++++++---- src/pathing/static.cpp | 34 ++++++---------------------------- src/ticks/fly_waypoints.cpp | 2 +- src/ticks/path_gen.cpp | 8 ++++---- 9 files changed, 44 insertions(+), 53 deletions(-) diff --git a/include/core/mission_state.hpp b/include/core/mission_state.hpp index 1f691bc7..e18fa4a8 100644 --- a/include/core/mission_state.hpp +++ b/include/core/mission_state.hpp @@ -44,8 +44,8 @@ class MissionState { void setInitPath(std::vector init_path); const std::vector& getInitPath(); - void setSearchPath(std::vector search_path); - const std::vector& getSearchPath(); + void setCoveragePath(std::vector coverage_path); + const std::vector& getCoveragePath(); /* * Gets a locking reference to the underlying tick for the given tick subclass T. @@ -109,7 +109,7 @@ class MissionState { std::mutex init_path_mut; // for reading/writing the initial path std::mutex search_path_mut; // for reading/writing the search path std::vector init_path; - std::vector search_path; + std::vector coverage_path; std::shared_ptr mav; std::shared_ptr airdrop; diff --git a/include/network/gcs_routes.hpp b/include/network/gcs_routes.hpp index af828631..8ddd779e 100644 --- a/include/network/gcs_routes.hpp +++ b/include/network/gcs_routes.hpp @@ -82,20 +82,21 @@ DEF_GCS_HANDLE(Post, airdrop); /* * GET /path/initial * - * TODO: reference protobuf class that encompasses the JSON - * --- - * Response is the cached initial path that hits all of the competition waypoints. - * - * { - * TODO: fill in the expected JSON output - * } - * * 200 OK: path was previously generated and returned * 404 NOT FOUND: no path has been generated yet * TODO: determine if there are more errors we might encounter */ DEF_GCS_HANDLE(Get, path, initial); +/* + * GET /path/coverage + * + * 200 OK: path was previously generated and returned + * 404 NOT FOUND: no path has been generated yet + * TODO: determine if there are more errors we might encounter + */ +DEF_GCS_HANDLE(Get, path, coverage); + /* * GET /path/initial/new * diff --git a/include/ticks/path_gen.hpp b/include/ticks/path_gen.hpp index ee6cadf9..825783ca 100644 --- a/include/ticks/path_gen.hpp +++ b/include/ticks/path_gen.hpp @@ -26,7 +26,7 @@ class PathGenTick : public Tick { Tick* tick() override; private: std::future> init_path; - std::future> search_path; + std::future> coverage_path; void startPathGeneration(); }; diff --git a/src/core/mission_state.cpp b/src/core/mission_state.cpp index 2fbbd89d..9a7858c8 100644 --- a/src/core/mission_state.cpp +++ b/src/core/mission_state.cpp @@ -76,14 +76,14 @@ const std::vector& MissionState::getInitPath() { return this->init_path; } -void MissionState::setSearchPath(std::vector search_path) { +void MissionState::setCoveragePath(std::vector coverage_path) { Lock lock(this->search_path_mut); - this->search_path = search_path; + this->coverage_path = coverage_path; } -const std::vector& MissionState::getSearchPath() { +const std::vector& MissionState::getCoveragePath() { Lock lock(this->search_path_mut); - return this->search_path; + return this->coverage_path; } std::shared_ptr MissionState::getMav() { return this->mav; } diff --git a/src/network/gcs.cpp b/src/network/gcs.cpp index 5a4149a6..040484f9 100644 --- a/src/network/gcs.cpp +++ b/src/network/gcs.cpp @@ -58,6 +58,7 @@ void GCSServer::_bindHandlers() { BIND_HANDLER(Post, mission); BIND_HANDLER(Post, airdrop); BIND_HANDLER(Get, path, initial); + BIND_HANDLER(Get, path, coverage); BIND_HANDLER(Get, path, initial, new); BIND_HANDLER(Post, path, initial, validate); BIND_HANDLER(Get, camera, capture); diff --git a/src/network/gcs_routes.cpp b/src/network/gcs_routes.cpp index d567a694..30fc2a0a 100644 --- a/src/network/gcs_routes.cpp +++ b/src/network/gcs_routes.cpp @@ -127,12 +127,23 @@ DEF_GCS_HANDLE(Post, airdrop) { DEF_GCS_HANDLE(Get, path, initial) { LOG_REQUEST("GET", "/path/initial"); - auto init_path = state->getInitPath(); - if (init_path.empty()) { + auto path = state->getInitPath(); + if (path.empty()) { LOG_RESPONSE(WARNING, "No initial path generated", BAD_REQUEST); } else { - auto init_path = state->getInitPath(); - std::string json = messagesToJson(init_path.begin(), init_path.end()); + std::string json = messagesToJson(path.begin(), path.end()); + LOG_RESPONSE(INFO, "Got initial path", OK, json.c_str(), mime::json); + } +} + +DEF_GCS_HANDLE(Get, path, coverage) { + LOG_REQUEST("GET", "/path/coverage"); + + auto path = state->getCoveragePath(); + if (path.empty()) { + LOG_RESPONSE(WARNING, "No coverage path generated", BAD_REQUEST); + } else { + std::string json = messagesToJson(path.begin(), path.end()); LOG_RESPONSE(INFO, "Got initial path", OK, json.c_str(), mime::json); } } diff --git a/src/pathing/static.cpp b/src/pathing/static.cpp index c3fa633c..8557a6e1 100644 --- a/src/pathing/static.cpp +++ b/src/pathing/static.cpp @@ -410,33 +410,10 @@ std::vector HoverCoveragePathing::run() { LOG_F(FATAL, "Hover airdrop pathing currently only supports 4 coordinates, not %lu", this->drop_zone.size()); } - XYZCoord top_left(std::numeric_limits::max(), std::numeric_limits::min(), 0); - XYZCoord top_right(std::numeric_limits::min(), std::numeric_limits::min(), 0); - XYZCoord bottom_left(std::numeric_limits::max(), std::numeric_limits::max(), 0); - XYZCoord bottom_right(std::numeric_limits::min(), std::numeric_limits::max(), 0); - - // thx chat gpt - for (const auto& coord : this->drop_zone) { - // Update top-left - if (coord.y > top_left.y || (coord.y == top_left.y && coord.x < top_left.x)) { - top_left = coord; - } - - // Update bottom-left - if (coord.y < bottom_left.y || (coord.y == bottom_left.y && coord.x < bottom_left.x)) { - bottom_left = coord; - } - - // Update top-right - if (coord.y > top_right.y || (coord.y == top_right.y && coord.x > top_right.x)) { - top_right = coord; - } - - // Update bottom-right - if (coord.y < bottom_right.y || (coord.y == bottom_right.y && coord.x > bottom_right.x)) { - bottom_right = coord; - } - } + XYZCoord top_left = this->drop_zone.at(3); + XYZCoord top_right = this->drop_zone.at(2); + XYZCoord bottom_left = this->drop_zone.at(0); + XYZCoord bottom_right = this->drop_zone.at(1); // assuming a rectangle!! @@ -451,7 +428,7 @@ std::vector HoverCoveragePathing::run() { double stop_x = top_right.x + (vision / 2.0); bool right = true; // start going from right to left - for (double y = start_y; y < stop_y; y += vision) { + for (double y = start_y; y > stop_y; y -= vision) { std::vector row; // row of points either from left to right or right to left for (double x = start_x; x < stop_x; x += vision) { row.push_back(XYZCoord(x, y, altitude)); @@ -560,6 +537,7 @@ std::vector generateSearchPath(std::shared_ptr state) { for (const XYZCoord& coord : pathing.run()) { coords.push_back(state->getCartesianConverter()->toLatLng(coord)); } + LOG_F(INFO, "Coverage path has %lu coordinates", coords.size()); return coords; } } diff --git a/src/ticks/fly_waypoints.cpp b/src/ticks/fly_waypoints.cpp index 3fe21a95..94be1729 100644 --- a/src/ticks/fly_waypoints.cpp +++ b/src/ticks/fly_waypoints.cpp @@ -19,7 +19,7 @@ Tick* FlyWaypointsTick::tick() { if (isMissionFinished) { return new MavUploadTick(this->state, new FlySearchTick(this->state), - state->getSearchPath(), false); + state->getCoveragePath(), false); } return nullptr; diff --git a/src/ticks/path_gen.cpp b/src/ticks/path_gen.cpp index 255dd377..ce076cd2 100644 --- a/src/ticks/path_gen.cpp +++ b/src/ticks/path_gen.cpp @@ -19,19 +19,19 @@ PathGenTick::PathGenTick(std::shared_ptr state) : Tick(state, Tick void PathGenTick::startPathGeneration() { this->init_path = std::async(std::launch::async, generateInitialPath, this->state); - this->search_path = std::async(std::launch::async, generateSearchPath, this->state); + this->coverage_path = std::async(std::launch::async, generateSearchPath, this->state); } std::chrono::milliseconds PathGenTick::getWait() const { return PATH_GEN_TICK_WAIT; } Tick* PathGenTick::tick() { auto init_status = this->init_path.wait_for(0ms); - auto search_status = this->search_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) { - LOG_F(INFO, "Initial path generated"); + LOG_F(INFO, "Initial and Coverage paths generated"); state->setInitPath(this->init_path.get()); - state->setSearchPath(this->search_path.get()); + state->setCoveragePath(this->coverage_path.get()); return new PathValidateTick(this->state); } From ebba5ddf0444c36c76b5aead8de7193683644383 Mon Sep 17 00:00:00 2001 From: Tyler Lentz Date: Tue, 11 Jun 2024 02:20:56 +0000 Subject: [PATCH 07/13] untested upload code --- include/core/mission_state.hpp | 15 +++--- include/network/mavlink.hpp | 6 +-- include/pathing/mission_path.hpp | 47 +++++++++++++++++++ include/pathing/static.hpp | 5 +- include/ticks/mav_upload.hpp | 5 +- include/ticks/path_gen.hpp | 5 +- include/utilities/serialize.hpp | 2 +- src/core/mission_state.cpp | 12 ++--- src/network/airdrop_client.cpp | 2 +- src/network/gcs_routes.cpp | 11 +++-- src/network/mavlink.cpp | 34 +++----------- src/pathing/CMakeLists.txt | 1 + src/pathing/mission_path.cpp | 79 ++++++++++++++++++++++++++++++++ src/pathing/static.cpp | 19 ++++---- src/ticks/mav_upload.cpp | 2 +- 15 files changed, 179 insertions(+), 66 deletions(-) create mode 100644 include/pathing/mission_path.hpp create mode 100644 src/pathing/mission_path.cpp diff --git a/include/core/mission_state.hpp b/include/core/mission_state.hpp index e18fa4a8..c3ddf370 100644 --- a/include/core/mission_state.hpp +++ b/include/core/mission_state.hpp @@ -17,6 +17,7 @@ #include "network/airdrop_client.hpp" #include "network/mavlink.hpp" #include "pathing/cartesian.hpp" +#include "pathing/mission_path.hpp" #include "protos/obc.pb.h" #include "ticks/ids.hpp" #include "utilities/constants.hpp" @@ -41,11 +42,11 @@ class MissionState { void setTick(Tick* newTick); - void setInitPath(std::vector init_path); - const std::vector& getInitPath(); + void setInitPath(const MissionPath& init_path); + MissionPath getInitPath(); - void setCoveragePath(std::vector coverage_path); - const std::vector& getCoveragePath(); + void setCoveragePath(const MissionPath& coverage_path); + MissionPath getCoveragePath(); /* * Gets a locking reference to the underlying tick for the given tick subclass T. @@ -107,9 +108,9 @@ class MissionState { std::shared_ptr tick; std::mutex init_path_mut; // for reading/writing the initial path - std::mutex search_path_mut; // for reading/writing the search path - std::vector init_path; - std::vector coverage_path; + MissionPath init_path; + std::mutex coverage_path_mut; // for reading/writing the coverage path + MissionPath coverage_path; std::shared_ptr mav; std::shared_ptr airdrop; diff --git a/include/network/mavlink.hpp b/include/network/mavlink.hpp index 104bafd0..1270415f 100644 --- a/include/network/mavlink.hpp +++ b/include/network/mavlink.hpp @@ -19,8 +19,8 @@ #include #include "protos/obc.pb.h" - #include "utilities/datatypes.hpp" +#include "pathing/mission_path.hpp" class MissionState; @@ -54,11 +54,11 @@ class MavlinkClient { * should never happen due to how the state machine is set up, but it is there just in case. */ bool uploadMissionUntilSuccess(std::shared_ptr state, - bool upload_geofence, std::vector waypoints) const; + bool upload_geofence, const MissionPath& waypoints) const; bool uploadGeofenceUntilSuccess(std::shared_ptr state) const; bool uploadWaypointsUntilSuccess(std::shared_ptr state, - std::vector waypoints) const; + const MissionPath& waypoints) const; std::pair latlng_deg(); double altitude_agl_m(); diff --git a/include/pathing/mission_path.hpp b/include/pathing/mission_path.hpp new file mode 100644 index 00000000..2d3bb7cc --- /dev/null +++ b/include/pathing/mission_path.hpp @@ -0,0 +1,47 @@ +#ifndef INCLUDE_PATHING_MISSION_PATH_HPP_ +#define INCLUDE_PATHING_MISSION_PATH_HPP_ + +#include + +#include +#include + +#include "utilities/datatypes.hpp" +#include "protos/obc.pb.h" + +/** + * Class which abstracts around a waypoint mission that we will upload via mavlink. + * + * There are two kinds of mission paths: forward paths and hover paths + * + * Forward paths will just be normal paths where you fly forward through all waypoints + * + * Hover paths will be where you hover at each waypoint for a specified duration + * + * It may make sense in the future to allow mixing these, so a mission path that contains + * both hovering and forward flight, but at this time it makes more sense to keep it + * simple with a binary choice on the entire path. + */ +class MissionPath { + public: + enum class Type { + FORWARD, HOVER + }; + + MissionPath(Type type, std::vector path, int hover_wait_s = 5); + MissionPath() = default; + + const std::vector& get() const; + const std::vector getCommands() const; + + private: + Type type; + std::vector path; + std::vector path_mav; + int hover_wait_s; + + void generateHoverCommands(); + void generateForwardCommands(); +}; + +#endif // INCLUDE_PATHING_MISSION_PATH_HPP_ diff --git a/include/pathing/static.hpp b/include/pathing/static.hpp index 30c26fd8..bcd70a01 100644 --- a/include/pathing/static.hpp +++ b/include/pathing/static.hpp @@ -15,6 +15,7 @@ #include "pathing/environment.hpp" #include "pathing/plotting.hpp" #include "pathing/tree.hpp" +#include "pathing/mission_path.hpp" #include "udp_squared/internal/enum.h" #include "utilities/constants.hpp" #include "utilities/datatypes.hpp" @@ -279,8 +280,8 @@ class AirdropApproachPathing { RRTPoint wind; }; -std::vector generateInitialPath(std::shared_ptr state); +MissionPath generateInitialPath(std::shared_ptr state); -std::vector generateSearchPath(std::shared_ptr state); +MissionPath generateSearchPath(std::shared_ptr state); #endif // INCLUDE_PATHING_STATIC_HPP_ diff --git a/include/ticks/mav_upload.hpp b/include/ticks/mav_upload.hpp index 1d68ce38..57101db4 100644 --- a/include/ticks/mav_upload.hpp +++ b/include/ticks/mav_upload.hpp @@ -11,6 +11,7 @@ #include "ticks/tick.hpp" #include "ticks/ids.hpp" #include "protos/obc.pb.h" +#include "pathing/mission_path.hpp" /* * Handles uploading waypoint mission to the Pixhawk flight @@ -21,7 +22,7 @@ class MavUploadTick: public Tick { public: MavUploadTick(std::shared_ptr state, - Tick* next_tick, std::vector waypoints, bool upload_geofence); + Tick* next_tick, const MissionPath& waypoints, bool upload_geofence); std::chrono::milliseconds getWait() const override; @@ -31,7 +32,7 @@ class MavUploadTick: public Tick { std::future mav_uploaded; Tick* next_tick; - std::vector waypoints; + MissionPath waypoints; bool upload_geofence; }; diff --git a/include/ticks/path_gen.hpp b/include/ticks/path_gen.hpp index 825783ca..0ff082b2 100644 --- a/include/ticks/path_gen.hpp +++ b/include/ticks/path_gen.hpp @@ -10,6 +10,7 @@ #include "ticks/tick.hpp" #include "core/mission_state.hpp" #include "protos/obc.pb.h" +#include "pathing/mission_path.hpp" /* * Generates a path, caches the path in the mission state, @@ -25,8 +26,8 @@ class PathGenTick : public Tick { Tick* tick() override; private: - std::future> init_path; - std::future> coverage_path; + std::future init_path; + std::future coverage_path; void startPathGeneration(); }; diff --git a/include/utilities/serialize.hpp b/include/utilities/serialize.hpp index 7d9f8f48..8df64950 100644 --- a/include/utilities/serialize.hpp +++ b/include/utilities/serialize.hpp @@ -24,7 +24,7 @@ std::string messagesToJson(Iterator begin, Iterator end) { std::string json = "["; while (it != end) { - google::protobuf::Message& message = *it; + const google::protobuf::Message& message = *it; std::string message_json; google::protobuf::util::MessageToJsonString(message, &message_json); diff --git a/src/core/mission_state.cpp b/src/core/mission_state.cpp index 9a7858c8..cfc5ca64 100644 --- a/src/core/mission_state.cpp +++ b/src/core/mission_state.cpp @@ -66,23 +66,23 @@ TickID MissionState::getTickID() { return this->tick->getID(); } -void MissionState::setInitPath(std::vector init_path) { +void MissionState::setInitPath(const MissionPath& init_path) { Lock lock(this->init_path_mut); this->init_path = init_path; } -const std::vector& MissionState::getInitPath() { +MissionPath MissionState::getInitPath() { Lock lock(this->init_path_mut); return this->init_path; } -void MissionState::setCoveragePath(std::vector coverage_path) { - Lock lock(this->search_path_mut); +void MissionState::setCoveragePath(const MissionPath& coverage_path) { + Lock lock(this->coverage_path_mut); this->coverage_path = coverage_path; } -const std::vector& MissionState::getCoveragePath() { - Lock lock(this->search_path_mut); +MissionPath MissionState::getCoveragePath() { + Lock lock(this->coverage_path_mut); return this->coverage_path; } diff --git a/src/network/airdrop_client.cpp b/src/network/airdrop_client.cpp index 6641fe41..ffbd9e77 100644 --- a/src/network/airdrop_client.cpp +++ b/src/network/airdrop_client.cpp @@ -49,7 +49,7 @@ void AirdropClient::_establishConnection() { while (true) { LOG_F(INFO, "Sending reset packets to all bottles..."); send_ad_packet(this->socket, makeResetPacket(UDP2_ALL)); - std::this_thread::sleep_for(2s); + std::this_thread::sleep_for(10s); if (stop) { return; } diff --git a/src/network/gcs_routes.cpp b/src/network/gcs_routes.cpp index 30fc2a0a..fc3aeb09 100644 --- a/src/network/gcs_routes.cpp +++ b/src/network/gcs_routes.cpp @@ -12,6 +12,7 @@ #include "utilities/serialize.hpp" #include "utilities/logging.hpp" #include "utilities/http.hpp" +#include "pathing/mission_path.hpp" #include "network/gcs_macros.hpp" #include "ticks/tick.hpp" #include "ticks/path_gen.hpp" @@ -128,10 +129,10 @@ DEF_GCS_HANDLE(Get, path, initial) { LOG_REQUEST("GET", "/path/initial"); auto path = state->getInitPath(); - if (path.empty()) { + if (path.get().empty()) { LOG_RESPONSE(WARNING, "No initial path generated", BAD_REQUEST); } else { - std::string json = messagesToJson(path.begin(), path.end()); + std::string json = messagesToJson(path.get().begin(), path.get().end()); LOG_RESPONSE(INFO, "Got initial path", OK, json.c_str(), mime::json); } } @@ -140,10 +141,10 @@ DEF_GCS_HANDLE(Get, path, coverage) { LOG_REQUEST("GET", "/path/coverage"); auto path = state->getCoveragePath(); - if (path.empty()) { + if (path.get().empty()) { LOG_RESPONSE(WARNING, "No coverage path generated", BAD_REQUEST); } else { - std::string json = messagesToJson(path.begin(), path.end()); + std::string json = messagesToJson(path.get().begin(), path.get().end()); LOG_RESPONSE(INFO, "Got initial path", OK, json.c_str(), mime::json); } } @@ -164,7 +165,7 @@ DEF_GCS_HANDLE(Get, path, initial, new) { DEF_GCS_HANDLE(Post, path, initial, validate) { LOG_REQUEST("POST", "/path/initial/validate"); - if (state->getInitPath().empty()) { + if (state->getInitPath().get().empty()) { LOG_RESPONSE(WARNING, "No initial path generated", BAD_REQUEST); return; } diff --git a/src/network/mavlink.cpp b/src/network/mavlink.cpp index 4aa4ab72..b73214e1 100644 --- a/src/network/mavlink.cpp +++ b/src/network/mavlink.cpp @@ -17,6 +17,7 @@ #include #include "core/mission_state.hpp" +#include "pathing/mission_path.hpp" #include "utilities/locks.hpp" #include "utilities/logging.hpp" @@ -131,14 +132,14 @@ MavlinkClient::MavlinkClient(std::string link) bool MavlinkClient::uploadMissionUntilSuccess(std::shared_ptr state, bool upload_geofence, - std::vector waypoints) const { + const MissionPath& path) const { if (upload_geofence) { if (!this->uploadGeofenceUntilSuccess(state)) { return false; } } - if (waypoints.size() > 0) { - if (!this->uploadWaypointsUntilSuccess(state, waypoints)) { + if (path.get().size() > 0) { + if (!this->uploadWaypointsUntilSuccess(state, path)) { return false; } } @@ -154,7 +155,7 @@ bool MavlinkClient::uploadGeofenceUntilSuccess(std::shared_ptr sta LOG_F(ERROR, "Upload failed - no mission"); return false; // todo determine return type } - if (state->getInitPath().empty()) { + if (state->getInitPath().get().empty()) { LOG_F(ERROR, "Upload failed - no initial path"); return false; } @@ -204,30 +205,9 @@ bool MavlinkClient::uploadGeofenceUntilSuccess(std::shared_ptr sta } bool MavlinkClient::uploadWaypointsUntilSuccess(std::shared_ptr state, - std::vector waypoints) const { + const MissionPath& waypoints) const { LOG_SCOPE_F(INFO, "Uploading waypoints"); - // Parse the waypoint information - std::vector mission_items; - int i = 0; - for (const auto& coord : waypoints) { - mavsdk::MissionRaw::MissionItem new_raw_item_nav{}; - new_raw_item_nav.seq = i; - new_raw_item_nav.frame = 3; // MAV_FRAME_GLOBAL_RELATIVE_ALT - new_raw_item_nav.command = 16; // MAV_CMD_NAV_WAYPOINT - new_raw_item_nav.current = (i == 0) ? 1 : 0; - new_raw_item_nav.autocontinue = 1; - new_raw_item_nav.param1 = 0.0; // Hold - new_raw_item_nav.param2 = 7.0; // Accept Radius 7.0m close to 25ft - new_raw_item_nav.param3 = 0.0; // Pass Radius - new_raw_item_nav.param4 = NAN; // Yaw - new_raw_item_nav.x = int32_t(std::round(coord.latitude() * 1e7)); - new_raw_item_nav.y = int32_t(std::round(coord.longitude() * 1e7)); - new_raw_item_nav.z = coord.altitude(); - new_raw_item_nav.mission_type = 0; // MAV_MISSION_TYPE_MISSION - mission_items.push_back(new_raw_item_nav); - i++; - } while (true) { LOG_F(INFO, "Sending waypoint information..."); @@ -236,7 +216,7 @@ bool MavlinkClient::uploadWaypointsUntilSuccess(std::shared_ptr st std::optional result{}; this->mission->upload_mission_async( - mission_items, [&result, &resultMut](const mavsdk::MissionRaw::Result& res) { + waypoints.getCommands(), [&result, &resultMut](const mavsdk::MissionRaw::Result& res) { resultMut.lock(); result = res; resultMut.unlock(); diff --git a/src/pathing/CMakeLists.txt b/src/pathing/CMakeLists.txt index deeb4f6d..cf467c65 100644 --- a/src/pathing/CMakeLists.txt +++ b/src/pathing/CMakeLists.txt @@ -6,6 +6,7 @@ set(FILES plotting.cpp static.cpp tree.cpp + mission_path.cpp ) set (LIB_DEPS diff --git a/src/pathing/mission_path.cpp b/src/pathing/mission_path.cpp new file mode 100644 index 00000000..5ed7c95a --- /dev/null +++ b/src/pathing/mission_path.cpp @@ -0,0 +1,79 @@ +#include "pathing/mission_path.hpp" + +#include + +#include "protos/obc.pb.h" +#include "utilities/logging.hpp" + +MissionPath::MissionPath(MissionPath::Type type, std::vector path, int hover_wait_s): + type(type), path(path), hover_wait_s(hover_wait_s) +{ + switch (type) { + case Type::HOVER: + generateHoverCommands(); + break; + default: + LOG_F(WARNING, "Unknown MissionPath type %d, defaulting to forward", static_cast(type)); + case Type::FORWARD: + generateForwardCommands(); + break; + } +} + +const std::vector& MissionPath::get() const { + return this->path; +} + +const std::vector MissionPath::getCommands() const { + return this->path_mav; +} + +void MissionPath::generateForwardCommands() { + this->path_mav.clear(); // incase this gets called multiple times, but it shouldnt + + // Parse the waypoint information + int i = 0; + for (const auto& coord : this->path) { + mavsdk::MissionRaw::MissionItem item{}; + item.seq = i; + item.frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; + item.command = MAV_CMD_NAV_WAYPOINT; + item.current = (i == 0) ? 1 : 0; + item.autocontinue = 1; + item.param1 = 0.0; // Hold + item.param2 = 7.0; // Accept Radius 7.0m close to 25ft + item.param3 = 0.0; // Pass Radius + item.param4 = NAN; // Yaw + item.x = int32_t(std::round(coord.latitude() * 1e7)); + item.y = int32_t(std::round(coord.longitude() * 1e7)); + item.z = coord.altitude(); + item.mission_type = MAV_MISSION_TYPE_MISSION; + this->path_mav.push_back(item); + i++; + } +} + +void MissionPath::generateHoverCommands() { + this->path_mav.clear(); + + // https://mavlink.io/en/messages/common.html#MAV_CMD_NAV_LOITER_TIME + int i = 0; + for (const auto& coord : this->path) { + mavsdk::MissionRaw::MissionItem item{}; + item.seq = i; + item.frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; + item.command = MAV_CMD_NAV_LOITER_TIME; + item.current = (i == 0) ? 1 : 0; + item.autocontinue = 1; + item.param1 = static_cast(this->hover_wait_s); + item.param2 = 0.0f; // 0 => dont need to point heading at next waypoint + item.param3 = 0.0f; // loiter radius, which shouldn't matter for hover quadplane + item.param4 = NAN; // xtrack, something for forward flight planes which we arent here + item.x = int32_t(std::round(coord.latitude() * 1e7)); + item.y = int32_t(std::round(coord.longitude() * 1e7)); + item.z = coord.altitude(); + item.mission_type = MAV_MISSION_TYPE_MISSION; + this->path_mav.push_back(item); + i++; + } +} diff --git a/src/pathing/static.cpp b/src/pathing/static.cpp index df48e100..8a989f2b 100644 --- a/src/pathing/static.cpp +++ b/src/pathing/static.cpp @@ -425,10 +425,10 @@ std::vector HoverCoveragePathing::run() { double vision = this->config.camera_vision_m; double altitude = this->config.altitude_m; - double start_y = top_left.y - (vision / 2.0); - double stop_y = bottom_left.y - (vision / 2.0); - double start_x = top_left.x + (vision / 2.0); - double stop_x = top_right.x + (vision / 2.0); + double start_y = std::max(top_left.y, top_right.y) - (vision / 2.0); + double stop_y = std::min(bottom_left.y, bottom_right.y) - (vision / 2.0); + double start_x = std::min(top_left.x, bottom_left.x) + (vision / 2.0); + double stop_x = std::max(top_right.x, bottom_right.x) + (vision / 2.0); bool right = true; // start going from right to left for (double y = start_y; y > stop_y; y -= vision) { @@ -489,7 +489,7 @@ RRTPoint AirdropApproachPathing::getDropLocation() const { return RRTPoint(drop_location, angle); } -std::vector generateInitialPath(std::shared_ptr state) { +MissionPath generateInitialPath(std::shared_ptr state) { // first waypoint is start // the other waypoitns is the goals @@ -527,19 +527,20 @@ std::vector generateInitialPath(std::shared_ptr state) { output_coords.push_back(state->getCartesianConverter()->toLatLng(wpt)); } - return output_coords; + return MissionPath(MissionPath::Type::FORWARD, output_coords); } -std::vector generateSearchPath(std::shared_ptr state) { +MissionPath 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 {}; + return MissionPath(MissionPath::Type::FORWARD, {}); } else { // hover HoverCoveragePathing pathing(state->mission_params.getAirdropBoundary(), state->config); std::vector coords; for (const XYZCoord &coord : pathing.run()) { coords.push_back(state->getCartesianConverter()->toLatLng(coord)); } - return coords; + return MissionPath(MissionPath::Type::HOVER, coords, + state->config.pathing.coverage.hover.hover_time_s); } } diff --git a/src/ticks/mav_upload.cpp b/src/ticks/mav_upload.cpp index b55fab62..3efebfcc 100644 --- a/src/ticks/mav_upload.cpp +++ b/src/ticks/mav_upload.cpp @@ -13,7 +13,7 @@ #include "protos/obc.pb.h" MavUploadTick::MavUploadTick(std::shared_ptr state, - Tick* next_tick, std::vector waypoints, bool upload_geofence) + Tick* next_tick, const MissionPath& waypoints, bool upload_geofence) :Tick(state, TickID::MavUpload), next_tick{next_tick}, waypoints{waypoints}, upload_geofence{upload_geofence} { this->mav_uploaded = std::async(std::launch::async, From 9118de1497ee91949aada153c58e479eed6e9ac5 Mon Sep 17 00:00:00 2001 From: Tyler Lentz Date: Tue, 11 Jun 2024 19:42:30 +0000 Subject: [PATCH 08/13] minor refactor to make init method in tick & fix mission not starting for flysearch --- include/ticks/active_takeoff.hpp | 3 +- include/ticks/fly_search.hpp | 2 ++ include/ticks/mav_upload.hpp | 1 + include/ticks/path_gen.hpp | 1 + include/ticks/tick.hpp | 15 ++++++++-- src/core/mission_state.cpp | 37 +++++++++++++++++++------ src/network/airdrop_client.cpp | 1 + src/network/mavlink.cpp | 47 +++----------------------------- src/pathing/mission_path.cpp | 2 +- src/ticks/active_takeoff.cpp | 16 +++++------ src/ticks/cv_loiter.cpp | 9 +++--- src/ticks/fly_search.cpp | 30 +++++++++++++------- src/ticks/mav_upload.cpp | 23 ++++++++++------ src/ticks/path_gen.cpp | 18 ++++++------ 14 files changed, 107 insertions(+), 98 deletions(-) diff --git a/include/ticks/active_takeoff.hpp b/include/ticks/active_takeoff.hpp index c397dd01..9c1246a5 100644 --- a/include/ticks/active_takeoff.hpp +++ b/include/ticks/active_takeoff.hpp @@ -19,11 +19,10 @@ class ActiveTakeoffTick: public Tick { std::chrono::milliseconds getWait() const override; + void init() override; Tick* tick() override; private: - bool started; - std::future takeoffResult; void armAndHover(); diff --git a/include/ticks/fly_search.hpp b/include/ticks/fly_search.hpp index 07d7388f..24413c8e 100644 --- a/include/ticks/fly_search.hpp +++ b/include/ticks/fly_search.hpp @@ -19,9 +19,11 @@ class FlySearchTick : public Tick { explicit FlySearchTick(std::shared_ptr state); std::chrono::milliseconds getWait() const override; + void init() override; Tick* tick() override; private: + bool mission_started; Polygon airdrop_boundary; std::chrono::milliseconds last_photo_time; }; diff --git a/include/ticks/mav_upload.hpp b/include/ticks/mav_upload.hpp index 57101db4..b9cf9d07 100644 --- a/include/ticks/mav_upload.hpp +++ b/include/ticks/mav_upload.hpp @@ -26,6 +26,7 @@ class MavUploadTick: public Tick { std::chrono::milliseconds getWait() const override; + void init() override; Tick* tick() override; private: diff --git a/include/ticks/path_gen.hpp b/include/ticks/path_gen.hpp index 0ff082b2..11a3489c 100644 --- a/include/ticks/path_gen.hpp +++ b/include/ticks/path_gen.hpp @@ -24,6 +24,7 @@ class PathGenTick : public Tick { std::chrono::milliseconds getWait() const override; + void init() override; Tick* tick() override; private: std::future init_path; diff --git a/include/ticks/tick.hpp b/include/ticks/tick.hpp index c8836132..5fc9cf53 100644 --- a/include/ticks/tick.hpp +++ b/include/ticks/tick.hpp @@ -22,11 +22,20 @@ class Tick { // how long to wait between running each tick function virtual std::chrono::milliseconds getWait() const = 0; - // function that is called every getWaitTimeMS() miliseconds - // return nullptr if no state change should happen - // return new implementation of Tick if state change should happen + /** + * function that is called every getWaitTimeMS() miliseconds + * return nullptr if no state change should happen + * return new implementation of Tick if state change should happen + */ virtual Tick* tick() = 0; + /** + * Code that should be run upon entering the tick should be placed here, not in the constructor + * because the constructor can possibly be called before actually transitioning into the tick, + * as is the case for + */ + virtual void init() {}; + constexpr TickID getID() const { return this->id; } constexpr const char* getName() const { return TICK_ID_TO_STR(this->id); } protected: diff --git a/src/core/mission_state.cpp b/src/core/mission_state.cpp index cfc5ca64..88ce6b4f 100644 --- a/src/core/mission_state.cpp +++ b/src/core/mission_state.cpp @@ -58,7 +58,10 @@ void MissionState::_setTick(Tick* newTick) { LOG_F(INFO, "%s -> %s", old_tick_name.c_str(), new_tick_name.c_str()); - tick.reset(newTick); + this->tick.reset(newTick); + if (newTick != nullptr) { + this->tick->init(); + } } TickID MissionState::getTickID() { @@ -86,18 +89,34 @@ MissionPath MissionState::getCoveragePath() { return this->coverage_path; } -std::shared_ptr MissionState::getMav() { return this->mav; } +std::shared_ptr MissionState::getMav() { + return this->mav; +} -void MissionState::setMav(std::shared_ptr mav) { this->mav = mav; } +void MissionState::setMav(std::shared_ptr mav) { + this->mav = mav; +} -std::shared_ptr MissionState::getAirdrop() { return this->airdrop; } +std::shared_ptr MissionState::getAirdrop() { + return this->airdrop; +} -void MissionState::setAirdrop(std::shared_ptr airdrop) { this->airdrop = airdrop; } +void MissionState::setAirdrop(std::shared_ptr airdrop) { + this->airdrop = airdrop; +} -std::shared_ptr MissionState::getCV() { return this->cv; } +std::shared_ptr MissionState::getCV() { + return this->cv; +} -void MissionState::setCV(std::shared_ptr cv) { this->cv = cv; } +void MissionState::setCV(std::shared_ptr cv) { + this->cv = cv; +} -std::shared_ptr MissionState::getCamera() { return this->camera; } +std::shared_ptr MissionState::getCamera() { + return this->camera; +} -void MissionState::setCamera(std::shared_ptr camera) { this->camera = camera; } +void MissionState::setCamera(std::shared_ptr camera) { + this->camera = camera; +} diff --git a/src/network/airdrop_client.cpp b/src/network/airdrop_client.cpp index ffbd9e77..b1f81378 100644 --- a/src/network/airdrop_client.cpp +++ b/src/network/airdrop_client.cpp @@ -46,6 +46,7 @@ void AirdropClient::_establishConnection() { std::atomic_bool stop = false; std::thread send_thread( [this, &stop]() { + loguru::set_thread_name("airdrop reset spam"); while (true) { LOG_F(INFO, "Sending reset packets to all bottles..."); send_ad_packet(this->socket, makeResetPacket(UDP2_ALL)); diff --git a/src/network/mavlink.cpp b/src/network/mavlink.cpp index b73214e1..fc68de7a 100644 --- a/src/network/mavlink.cpp +++ b/src/network/mavlink.cpp @@ -375,8 +375,8 @@ bool MavlinkClient::armAndHover(std::shared_ptr state) { return false; } - // TODO: config option for this const float TAKEOFF_ALT = state->config.takeoff.altitude_m; + LOG_F(INFO, "Setting takeoff altitude to %f", TAKEOFF_ALT); auto r1 = this->action->set_takeoff_altitude(TAKEOFF_ALT); if (r1 != mavsdk::Action::Result::Success) { LOG_S(ERROR) << "FAIL: could not set takeoff alt " << r1; @@ -386,31 +386,12 @@ bool MavlinkClient::armAndHover(std::shared_ptr state) { LOG_S(INFO) << "Queried takeoff alt to be " << alt_checked << " with status " << r2; LOG_F(INFO, "Attempting to take off to %fm...", TAKEOFF_ALT); - const mavsdk::Action::Result takeoff_result = this->action->takeoff(); + auto takeoff_result = this->action->takeoff(); if (takeoff_result != mavsdk::Action::Result::Success) { LOG_F(ERROR, "Take off failed"); return false; } - // need to figure out correct values to send for a VTOL takeoff command - // auto result = this->passthrough->send_command_int(mavsdk::MavlinkPassthrough::CommandInt { - // .target_sysid = this->passthrough->get_our_sysid(), - // .target_compid = this->passthrough->get_our_compid(), - // .command = MAV_CMD_NAV_VTOL_TAKEOFF, - // .frame = MAV_FRAME_GLOBAL_RELATIVE_ALT, - // .param1 = 0, - // .param2 = VTOL_TRANSITION_HEADING_NEXT_WAYPOINT, // unsure if arduplane will respect this - // .param3 = 0, - // .param4 = NAN, - // .x = 0, - // .y = 0, - // .z = TAKEOFF_ALT // currently hard coded to 30m (~100ft) - // }); - // if (result != mavsdk::MavlinkPassthrough::Result::Success) { - // LOG_S(ERROR) << "FAIL: takeoff cmd not accepted because " << result; - // // return false; - // } - LOG_F(INFO, "Waiting to reach target altitude of %f", TAKEOFF_ALT); float current_position = 0; while (current_position < TAKEOFF_ALT) { @@ -424,36 +405,16 @@ bool MavlinkClient::armAndHover(std::shared_ptr state) { } /** - * Starts the mavlink mission and transitions vehicle from VTOL to fix wing. + * Starts the currently uploaded mavlink mission */ bool MavlinkClient::startMission() { - LOG_F(INFO, "Attempting to start mission..."); - LOG_F(INFO, "Querying target takeoff altitude"); - const auto& [takeoff_result, target_alt] = this->action->get_takeoff_altitude(); - if (takeoff_result != mavsdk::Action::Result::Success) { - LOG_S(ERROR) << "FAIL: could not query takeoff altitude: " << target_alt; - return false; - } - LOG_F(INFO, "Target takeoff altitude is %f", target_alt); - - float current_position = this->telemetry->position().relative_altitude_m; - - LOG_F(INFO, "Checking target altitude"); - if (current_position < target_alt) { - LOG_F(ERROR, "FAIL: Vehicle has not reached desired altitude (%f < %f)", current_position, - target_alt); - return false; - } - - LOG_F(INFO, "About to send start command"); + LOG_F(INFO, "Sending start mission command"); auto start_result = this->mission->start_mission(); if (start_result != mavsdk::MissionRaw::Result::Success) { LOG_S(ERROR) << "FAIL: Mission could not start " << start_result; return false; } - // will by default transition to forward flight - LOG_F(INFO, "Mission Started!"); return true; } diff --git a/src/pathing/mission_path.cpp b/src/pathing/mission_path.cpp index 5ed7c95a..762cfc3c 100644 --- a/src/pathing/mission_path.cpp +++ b/src/pathing/mission_path.cpp @@ -68,7 +68,7 @@ void MissionPath::generateHoverCommands() { item.param1 = static_cast(this->hover_wait_s); item.param2 = 0.0f; // 0 => dont need to point heading at next waypoint item.param3 = 0.0f; // loiter radius, which shouldn't matter for hover quadplane - item.param4 = NAN; // xtrack, something for forward flight planes which we arent here + item.param4 = 0.0f; // xtrack, something for forward flight planes which we arent here item.x = int32_t(std::round(coord.latitude() * 1e7)); item.y = int32_t(std::round(coord.longitude() * 1e7)); item.z = coord.altitude(); diff --git a/src/ticks/active_takeoff.cpp b/src/ticks/active_takeoff.cpp index 4c4004a7..2eb6ebc4 100644 --- a/src/ticks/active_takeoff.cpp +++ b/src/ticks/active_takeoff.cpp @@ -8,27 +8,25 @@ #include "ticks/fly_waypoints.hpp" #include "ticks/mission_prep.hpp" -ActiveTakeoffTick::ActiveTakeoffTick(std::shared_ptr state) - :Tick(state, TickID::ActiveTakeoff) { - this->started = false; +ActiveTakeoffTick::ActiveTakeoffTick(std::shared_ptr state): + Tick(state, TickID::ActiveTakeoff) +{ } std::chrono::milliseconds ActiveTakeoffTick::getWait() const { return ACTIVE_TAKEOFF_TICK_WAIT; } +void ActiveTakeoffTick::init() { + this->armAndHover(); +} + void ActiveTakeoffTick::armAndHover() { this->takeoffResult = std::async(std::launch::async, [this](){ return this->state->getMav()->armAndHover(this->state); }); } Tick* ActiveTakeoffTick::tick() { - if (!started) { - started = true; - this->armAndHover(); - return nullptr; - } - auto takeoff = this->takeoffResult.wait_for(std::chrono::milliseconds(0)); if (takeoff != std::future_status::ready) { diff --git a/src/ticks/cv_loiter.cpp b/src/ticks/cv_loiter.cpp index dec16f2a..b3ab554d 100644 --- a/src/ticks/cv_loiter.cpp +++ b/src/ticks/cv_loiter.cpp @@ -8,10 +8,11 @@ #include "ticks/fly_search.hpp" -CVLoiterTick::CVLoiterTick(std::shared_ptr state) - :Tick(state, TickID::CVLoiter) { - status = CVLoiterTick::Status::None; - } +CVLoiterTick::CVLoiterTick(std::shared_ptr state): + Tick(state, TickID::CVLoiter) +{ + this->status = CVLoiterTick::Status::None; +} std::chrono::milliseconds CVLoiterTick::getWait() const { return CV_LOITER_TICK_WAIT; diff --git a/src/ticks/fly_search.cpp b/src/ticks/fly_search.cpp index ed82f2fb..b9bd0a03 100644 --- a/src/ticks/fly_search.cpp +++ b/src/ticks/fly_search.cpp @@ -9,19 +9,29 @@ using namespace std::chrono_literals; // NOLINT -FlySearchTick::FlySearchTick(std::shared_ptr state) - :Tick(state, TickID::FlySearch) { - this->state->getCamera()->startStreaming(); - this->airdrop_boundary = this->state->mission_params.getAirdropBoundary(); - this->last_photo_time = getUnixTime_ms(); - } +FlySearchTick::FlySearchTick(std::shared_ptr state): + Tick(state, TickID::FlySearch) +{ + this->mission_started = false; +} std::chrono::milliseconds FlySearchTick::getWait() const { return FLY_SEARCH_TICK_WAIT; } +void FlySearchTick::init() { + 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(); +} + Tick* FlySearchTick::tick() { - // TODO: Eventually implement dynamic avoidance so we dont crash brrr + if (!this->mission_started) { + this->mission_started = this->state->getMav()->startMission(); + return nullptr; + } bool isMissionFinished = state->getMav()->isMissionFinished(); @@ -31,9 +41,9 @@ Tick* FlySearchTick::tick() { return new CVLoiterTick(this->state); } - // TODO: Should replace these pairs with GPSCords - std::pair latlng = state->getMav()->latlng_deg(); - bool isAboveFlightzone = this->state->getMav()->isPointInPolygon(latlng, this->airdrop_boundary); //NOLINT + auto latlng = state->getMav()->latlng_deg(); + bool isAboveFlightzone = + this->state->getMav()->isPointInPolygon(latlng, this->airdrop_boundary); // Checks if the MAV is above the flight zone and // if the time since the last photo is greater than 1 second diff --git a/src/ticks/mav_upload.cpp b/src/ticks/mav_upload.cpp index 3efebfcc..bb04a113 100644 --- a/src/ticks/mav_upload.cpp +++ b/src/ticks/mav_upload.cpp @@ -13,21 +13,26 @@ #include "protos/obc.pb.h" MavUploadTick::MavUploadTick(std::shared_ptr state, - Tick* next_tick, const MissionPath& waypoints, bool upload_geofence) - :Tick(state, TickID::MavUpload), next_tick{next_tick}, - waypoints{waypoints}, upload_geofence{upload_geofence} { - this->mav_uploaded = std::async(std::launch::async, - &MavlinkClient::uploadMissionUntilSuccess, - this->state->getMav(), - this->state, - upload_geofence, - waypoints); + Tick* next_tick, const MissionPath& waypoints, bool upload_geofence): + Tick(state, TickID::MavUpload), next_tick{next_tick}, + waypoints{waypoints}, upload_geofence{upload_geofence} +{ + } std::chrono::milliseconds MavUploadTick::getWait() const { return MAV_UPLOAD_TICK_WAIT; } +void MavUploadTick::init() { + this->mav_uploaded = std::async(std::launch::async, + &MavlinkClient::uploadMissionUntilSuccess, + this->state->getMav(), + this->state, + upload_geofence, + waypoints); +} + Tick* MavUploadTick::tick() { auto status = this->mav_uploaded.wait_for(std::chrono::milliseconds(0)); if (status == std::future_status::ready) { diff --git a/src/ticks/path_gen.cpp b/src/ticks/path_gen.cpp index ce076cd2..02cca714 100644 --- a/src/ticks/path_gen.cpp +++ b/src/ticks/path_gen.cpp @@ -13,17 +13,14 @@ using namespace std::chrono_literals; -PathGenTick::PathGenTick(std::shared_ptr state) : Tick(state, TickID::PathGen) { - startPathGeneration(); -} - -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); -} +PathGenTick::PathGenTick(std::shared_ptr state) : Tick(state, TickID::PathGen) {} std::chrono::milliseconds PathGenTick::getWait() const { return PATH_GEN_TICK_WAIT; } +void PathGenTick::init() { + startPathGeneration(); +} + Tick* PathGenTick::tick() { auto init_status = this->init_path.wait_for(0ms); auto search_status = this->coverage_path.wait_for(0ms); @@ -37,3 +34,8 @@ Tick* PathGenTick::tick() { return nullptr; } + +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); +} \ No newline at end of file From 3a0cf8b507d67cc1690586f62583c67559ea7e00 Mon Sep 17 00:00:00 2001 From: Tyler Lentz Date: Tue, 11 Jun 2024 22:45:39 +0000 Subject: [PATCH 09/13] better coverage path & lots of small things --- configs/config.json | 4 +- configs/dev-config.json | 4 +- include/network/mavlink.hpp | 5 +- include/pathing/environment.hpp | 9 +++- include/pathing/static.hpp | 3 +- include/ticks/fly_search.hpp | 1 + src/network/mavlink.cpp | 84 ++++++++++++++------------------- src/pathing/environment.cpp | 2 +- src/pathing/mission_path.cpp | 23 ++++++++- src/pathing/static.cpp | 84 ++++++++++++++++++++++++++------- src/ticks/fly_search.cpp | 30 +++++++----- src/utilities/common.cpp | 3 ++ 12 files changed, 164 insertions(+), 88 deletions(-) diff --git a/configs/config.json b/configs/config.json index 054d6a65..1bf1b28e 100644 --- a/configs/config.json +++ b/configs/config.json @@ -27,11 +27,11 @@ }, "coverage": { "altitude_m": 30.0, - "camera_vision_m": 20, + "camera_vision_m": 6.0, "method": "hover", "hover": { "pictures_per_stop": 1, - "hover_time_s": 5 + "hover_time_s": 1 }, "forward": { "optimize": true, diff --git a/configs/dev-config.json b/configs/dev-config.json index d981e940..717b78a4 100644 --- a/configs/dev-config.json +++ b/configs/dev-config.json @@ -27,11 +27,11 @@ }, "coverage": { "altitude_m": 30.0, - "camera_vision_m": 20, + "camera_vision_m": 6.0, "method": "hover", "hover": { "pictures_per_stop": 1, - "hover_time_s": 5 + "hover_time_s": 1 }, "forward": { "optimize": true, diff --git a/include/network/mavlink.hpp b/include/network/mavlink.hpp index 1270415f..b718c594 100644 --- a/include/network/mavlink.hpp +++ b/include/network/mavlink.hpp @@ -7,6 +7,7 @@ #include #include #include +#include #include #include #include @@ -71,8 +72,7 @@ class MavlinkClient { double roll_deg(); bool isArmed(); mavsdk::Telemetry::FlightMode flight_mode(); - double angle2D(double x1, double y1, double x2, double y2); - bool isPointInPolygon(std::pair latlng, std::vector region); + int32_t curr_waypoint() const; bool isMissionFinished(); mavsdk::Telemetry::RcStatus get_conn_status(); bool armAndHover(std::shared_ptr state); @@ -86,6 +86,7 @@ class MavlinkClient { std::unique_ptr geofence; std::unique_ptr action; std::unique_ptr passthrough; + std::unique_ptr param; struct Data { double lat_deg {}; diff --git a/include/pathing/environment.hpp b/include/pathing/environment.hpp index d95d8bf3..ac21105c 100644 --- a/include/pathing/environment.hpp +++ b/include/pathing/environment.hpp @@ -101,7 +101,12 @@ class Environment { * on the edge are counted as outside the polygon (to be more * conservative) * - * Public ONLY for the sake of testing + * Public ONLY for the sake of testing <-- no (read below) + * + * Making this static so that other parts of the code can access it + * but really this should just be a detacted helper function, should + * refactor this eventually, but for rn static is the easy thing to do + * - tyler * * @param point ==> given point * @return ==> whether or not the point is in this polygon object @@ -109,7 +114,7 @@ class Environment { * [TODO] make a method to augment the polygon to get similar polygons * [TODO] something that increases cost based on time in the edge */ - bool isPointInPolygon(const Polygon& polygon, const XYZCoord& point) const; + static bool isPointInPolygon(const Polygon& polygon, const XYZCoord& point); /** * Checks wheter a line segment is in bounds or not, it must NOT intersect diff --git a/include/pathing/static.hpp b/include/pathing/static.hpp index bcd70a01..cc489b9d 100644 --- a/include/pathing/static.hpp +++ b/include/pathing/static.hpp @@ -244,11 +244,12 @@ class ForwardCoveragePathing { */ class HoverCoveragePathing { public: - HoverCoveragePathing(Polygon drop_zone, const OBCConfig& config); + explicit HoverCoveragePathing(std::shared_ptr state); std::vector run(); private: + std::shared_ptr state; AirdropCoverageConfig config; Polygon drop_zone; }; diff --git a/include/ticks/fly_search.hpp b/include/ticks/fly_search.hpp index 24413c8e..b8e50280 100644 --- a/include/ticks/fly_search.hpp +++ b/include/ticks/fly_search.hpp @@ -23,6 +23,7 @@ class FlySearchTick : public Tick { Tick* tick() override; private: + std::size_t curr_mission_item; bool mission_started; Polygon airdrop_boundary; std::chrono::milliseconds last_photo_time; diff --git a/src/network/mavlink.cpp b/src/network/mavlink.cpp index fc68de7a..ae7a3e6e 100644 --- a/src/network/mavlink.cpp +++ b/src/network/mavlink.cpp @@ -2,6 +2,7 @@ #include #include +#include #include #include #include @@ -37,13 +38,16 @@ MavlinkClient::MavlinkClient(std::string link) LOG_S(WARNING) << "Mavlink connection failed: " << conn_result << ". Trying again in 5 seconds..."; - std::this_thread::sleep_for(std::chrono::seconds(5)); + std::this_thread::sleep_for(5s); } + // it wont have gotten a heartbeat immediately so just give it a moment so we don't + // have to wait the 3s every time + std::this_thread::sleep_for(100ms); // Wait for the system to connect via heartbeat while (mavsdk.systems().size() == 0) { LOG_F(WARNING, "No heartbeat. Trying again in 3 seconds..."); - std::this_thread::sleep_for(std::chrono::seconds(3)); + std::this_thread::sleep_for(3s); } LOG_F(INFO, "Mavlink heartbeat received"); @@ -57,6 +61,26 @@ MavlinkClient::MavlinkClient(std::string link) this->geofence = std::make_unique(system); this->action = std::make_unique(system); this->passthrough = std::make_unique(system); + this->param = std::make_unique(system); + + // need to make sure this parameter is enabled so that during the search phase + // it stops to loiter at the loiter points and doesn't just stay in fixed wing mode + // the whole time + LOG_F(INFO, "Setting Q_GUIDED_MODE to 1..."); + while (true) { + auto result = this->param->set_param_int("Q_GUIDED_MODE", 1); + if (result != mavsdk::Param::Result::Success) { + LOG_S(ERROR) << "Failed to set Q_GUIDED_MODE: " << result; + std::this_thread::sleep_for(1s); + continue; + } + LOG_F(INFO, "Set Q_GUIDED_MODE to %d", 1); + break; + } + + LOG_F(INFO, "Logging out all mavlink params at TRACE level..."); + auto all_params = this->param->get_all_params(); + VLOG_S(TRACE) << all_params; // Set position update rate (1 Hz) // TODO: set the 1.0 update rate value in the obc config @@ -73,7 +97,7 @@ MavlinkClient::MavlinkClient(std::string link) LOG_S(WARNING) << "Setting mavlink polling rate to 1.0 failed: " << set_rate_result << ". Trying again..."; - std::this_thread::sleep_for(std::chrono::seconds(1)); + std::this_thread::sleep_for(1s); } LOG_F(INFO, "Setting mavlink telemetry subscriptions"); @@ -300,49 +324,8 @@ mavsdk::Telemetry::FlightMode MavlinkClient::flight_mode() { return this->data.flight_mode; } -/** - * Helper function that goes along with isPointInPolygon(). - */ -double MavlinkClient::angle2D(double x1, double y1, double x2, double y2) { - double dtheta, theta1, theta2; - - theta1 = atan2(y1, x1); - theta2 = atan2(y2, x2); - dtheta = theta2 - theta1; - - while (dtheta > M_PI) { - dtheta -= 2 * M_PI; - } - while (dtheta < -M_PI) { - dtheta += 2 * M_PI; - } - - return (dtheta); -} - -/** - * This function checks if a coordiante is inside a polygon region. - * Source: - * https://stackoverflow.com/questions/4287780/detecting-whether-a-gps-coordinate-falls-within-a-polygon-on-a-map - */ -bool MavlinkClient::isPointInPolygon(std::pair latlng, - std::vector region) { - int n = region.size(); - double angle = 0; - - for (int i = 0; i < n; i++) { - double point_1_lat = region[i].x - latlng.first; - double point_1_lng = region[i].y - latlng.second; - double point_2_lat = region[(i + 1) % n].x - latlng.first; - double point_2_lng = region[(i + 1) % n].y - latlng.second; - angle += MavlinkClient::angle2D(point_1_lat, point_1_lng, point_2_lat, point_2_lng); - } - - if (std::abs(angle) < M_PI) { - return false; - } - - return true; +int32_t MavlinkClient::curr_waypoint() const { + return this->mission->mission_progress().current; } bool MavlinkClient::isMissionFinished() { @@ -376,8 +359,13 @@ bool MavlinkClient::armAndHover(std::shared_ptr state) { } const float TAKEOFF_ALT = state->config.takeoff.altitude_m; - LOG_F(INFO, "Setting takeoff altitude to %f", TAKEOFF_ALT); - auto r1 = this->action->set_takeoff_altitude(TAKEOFF_ALT); + // for some reason on the sitl sometimes it gets to right below the takeoff + // alt but then never reaches the exact value, so this is a hack to tell it + // to takeoff to 1m above where we actually want to takeoff so we reach + // the non adjusted altitude + const float TAKEOFF_ALT_ADJ = TAKEOFF_ALT + 1; + LOG_F(INFO, "Setting takeoff altitude to %f", TAKEOFF_ALT_ADJ); + auto r1 = this->action->set_takeoff_altitude(TAKEOFF_ALT_ADJ); if (r1 != mavsdk::Action::Result::Success) { LOG_S(ERROR) << "FAIL: could not set takeoff alt " << r1; return false; diff --git a/src/pathing/environment.cpp b/src/pathing/environment.cpp index ae4e8012..c33efbce 100644 --- a/src/pathing/environment.cpp +++ b/src/pathing/environment.cpp @@ -136,7 +136,7 @@ XYZCoord Environment::getRandomPoint() const { int Environment::getNumGoals() const { return goals.size(); } -bool Environment::isPointInPolygon(const Polygon& polygon, const XYZCoord& point) const { +bool Environment::isPointInPolygon(const Polygon& polygon, const XYZCoord& point) { bool is_inside = false; // point in polygon diff --git a/src/pathing/mission_path.cpp b/src/pathing/mission_path.cpp index 762cfc3c..9f9bf492 100644 --- a/src/pathing/mission_path.cpp +++ b/src/pathing/mission_path.cpp @@ -56,6 +56,27 @@ void MissionPath::generateForwardCommands() { void MissionPath::generateHoverCommands() { this->path_mav.clear(); + + + // first thing first we have to actually transition the plane back into multicopter mode + // because it will by default attempt to be in forward flight during the mission + // https://ardupilot.org/plane/docs/common-mavlink-mission-command-messages-mav_cmd.html#mav-cmd-do-vtol-transition NOLINT + // mavsdk::MissionRaw::MissionItem transition; + // transition.seq = 0; + // transition.frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; + // transition.command = MAV_CMD_DO_VTOL_TRANSITION; + // transition.current = 1; // starting item, so current is 1 + // transition.autocontinue = 1; + // transition.param1 = MAV_VTOL_STATE_MC; // multicopter/VTOL mode + // transition.param2 = 0.0f; + // transition.param3 = 0.0f; + // transition.param4 = 0.0f; + // transition.x = 0.0f; // not used in this command + // transition.y = 0.0f; + // transition.z = 0.0f; + // transition.mission_type = MAV_MISSION_TYPE_MISSION; + // this->path_mav.push_back(transition); + // https://mavlink.io/en/messages/common.html#MAV_CMD_NAV_LOITER_TIME int i = 0; for (const auto& coord : this->path) { @@ -63,7 +84,7 @@ void MissionPath::generateHoverCommands() { item.seq = i; item.frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; item.command = MAV_CMD_NAV_LOITER_TIME; - item.current = (i == 0) ? 1 : 0; + item.current = (i == 0) ? 1 : 0; // first waypoint should be true, others false item.autocontinue = 1; item.param1 = static_cast(this->hover_wait_s); item.param2 = 0.0f; // 0 => dont need to point heading at next waypoint diff --git a/src/pathing/static.cpp b/src/pathing/static.cpp index 8a989f2b..1f44445a 100644 --- a/src/pathing/static.cpp +++ b/src/pathing/static.cpp @@ -19,6 +19,7 @@ #include "utilities/datatypes.hpp" #include "utilities/obc_config.hpp" #include "utilities/rng.hpp" +#include "utilities/common.hpp" RRT::RRT(RRTPoint start, std::vector goals, double search_radius, Polygon bounds, const OBCConfig &config, std::vector obstacles, std::vector angles) @@ -401,9 +402,50 @@ std::vector ForwardCoveragePathing::generatePath( return path; } -HoverCoveragePathing::HoverCoveragePathing(Polygon drop_zone, const OBCConfig& config): - config{config.pathing.coverage}, drop_zone{drop_zone} -{} +HoverCoveragePathing::HoverCoveragePathing(std::shared_ptr state): + config{state->config.pathing.coverage}, + drop_zone{state->mission_params.getAirdropBoundary()}, + state{state} +{ + +} + +// Function to calculate the center of the polygon +XYZCoord calculateCenter(const std::vector& polygon) { + XYZCoord center(0.0, 0.0, 0.0); + for (const auto& point : polygon) { + center.x += point.x; + center.y += point.y; + } + center.x /= polygon.size(); + center.y /= polygon.size(); + return center; +} + +// Function to scale the polygon +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) { + point.x -= center.x; + point.y -= center.y; + } + + // Step 3: Scale the 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) { + point.x += center.x; + point.y += center.y; + } +} + std::vector HoverCoveragePathing::run() { if (this->drop_zone.size() != 4) { @@ -418,8 +460,6 @@ std::vector HoverCoveragePathing::run() { XYZCoord bottom_left = this->drop_zone.at(0); XYZCoord bottom_right = this->drop_zone.at(1); - // assuming a rectangle!! - std::vector hover_points; double vision = this->config.camera_vision_m; @@ -430,11 +470,17 @@ std::vector HoverCoveragePathing::run() { double start_x = std::min(top_left.x, bottom_left.x) + (vision / 2.0); double stop_x = std::max(top_right.x, bottom_right.x) + (vision / 2.0); + Polygon scaled_drop_zone = this->drop_zone; + scalePolygon(scaled_drop_zone, 1.20); + bool right = true; // start going from right to left for (double y = start_y; y > stop_y; y -= vision) { std::vector row; // row of points either from left to right or right to left for (double x = start_x; x < stop_x; x += vision) { - row.push_back(XYZCoord(x, y, altitude)); + XYZCoord pt(x, y, altitude); + if (Environment::isPointInPolygon(scaled_drop_zone, pt)) { + row.push_back(pt); + } } if (!right) { std::reverse(row.begin(), row.end()); @@ -449,13 +495,13 @@ std::vector HoverCoveragePathing::run() { AirdropApproachPathing::AirdropApproachPathing(const RRTPoint &start, const XYZCoord &goal, RRTPoint wind, Polygon bounds, const OBCConfig &config, - std::vector obstacles) - : start(start), - goal(goal), - wind(wind), - airspace(Environment(bounds, {}, {goal}, obstacles)), - dubins(Dubins(config.pathing.dubins.turning_radius, config.pathing.dubins.point_separation)), - config(config) {} + std::vector obstacles): + start(start), + goal(goal), + wind(wind), + airspace(Environment(bounds, {}, {goal}, obstacles)), + dubins(Dubins(config.pathing.dubins.turning_radius, config.pathing.dubins.point_separation)), + config(config) {} std::vector AirdropApproachPathing::run() const { RRTPoint drop_vector = getDropLocation(); @@ -535,12 +581,14 @@ MissionPath generateSearchPath(std::shared_ptr state) { LOG_F(FATAL, "Forward search path not fully integrated yet."); return MissionPath(MissionPath::Type::FORWARD, {}); } else { // hover - HoverCoveragePathing pathing(state->mission_params.getAirdropBoundary(), state->config); - std::vector coords; - for (const XYZCoord &coord : pathing.run()) { - coords.push_back(state->getCartesianConverter()->toLatLng(coord)); + 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, coords, + + return MissionPath(MissionPath::Type::HOVER, gps_coords, state->config.pathing.coverage.hover.hover_time_s); } } diff --git a/src/ticks/fly_search.cpp b/src/ticks/fly_search.cpp index b9bd0a03..a71441a6 100644 --- a/src/ticks/fly_search.cpp +++ b/src/ticks/fly_search.cpp @@ -6,6 +6,7 @@ #include "ticks/ids.hpp" #include "utilities/common.hpp" #include "ticks/cv_loiter.hpp" +#include "pathing/environment.hpp" using namespace std::chrono_literals; // NOLINT @@ -13,6 +14,9 @@ FlySearchTick::FlySearchTick(std::shared_ptr state): Tick(state, TickID::FlySearch) { this->mission_started = false; + this->curr_mission_item = 1; // if this was 0 it would take a picture immediately after + // entering the search mission, so set to 1 so it doesn't start taking pictures until + // actually over the search zone } std::chrono::milliseconds FlySearchTick::getWait() const { @@ -41,19 +45,23 @@ Tick* FlySearchTick::tick() { return new CVLoiterTick(this->state); } - auto latlng = state->getMav()->latlng_deg(); - bool isAboveFlightzone = - this->state->getMav()->isPointInPolygon(latlng, this->airdrop_boundary); + // IMPORTANT: currently hardcoded to assume hover search pathing, so it + // takes photos whenever it gets to a new waypoint (loiter position) + // if we were doing forward pathing would probably want to make it + // take photos at an interval but only when over the zone + auto curr_waypoint = this->state->getMav()->curr_waypoint(); + if (this->curr_mission_item != curr_waypoint) { + for (int i = 0; i < this->state->config.pathing.coverage.hover.pictures_per_stop; i++) { + auto photo = this->state->getCamera()->takePicture(500ms, this->state->getMav()); - // Checks if the MAV is above the flight zone and - // if the time since the last photo is greater than 1 second - if (isAboveFlightzone && ((getUnixTime_ms() - this->last_photo_time) > 1000ms)) { - auto photo = this->state->getCamera()->takePicture(1000ms, this->state->getMav()); - - if (photo.has_value()) { - this->last_photo_time = getUnixTime_ms(); // Update the last photo time - this->state->getCV()->runPipeline(photo.value()); // Run the pipeline on the photo + if (photo.has_value()) { + this->last_photo_time = getUnixTime_ms(); // Update the last photo time + this->state->getCV()->runPipeline(photo.value()); // Run the pipeline on the photo + } } + this->curr_mission_item = curr_waypoint; + + return nullptr; } return nullptr; diff --git a/src/utilities/common.cpp b/src/utilities/common.cpp index 8da2a6b0..6dc8c15f 100644 --- a/src/utilities/common.cpp +++ b/src/utilities/common.cpp @@ -1,7 +1,10 @@ #include "utilities/common.hpp" #include +#include +#include +#include "utilities/datatypes.hpp" std::chrono::seconds getUnixTime_s() { const auto now = std::chrono::system_clock::now(); From f610e0fdd73f9cd6f55cdcb4ca10781e04db952c Mon Sep 17 00:00:00 2001 From: Tyler Lentz Date: Tue, 11 Jun 2024 22:49:19 +0000 Subject: [PATCH 10/13] fix lint --- include/pathing/mission_path.hpp | 4 ++-- include/pathing/tree.hpp | 1 - include/ticks/tick.hpp | 3 ++- src/pathing/mission_path.cpp | 8 ++++---- src/pathing/static.cpp | 17 +++++++---------- src/ticks/active_takeoff.cpp | 4 +--- src/ticks/cv_loiter.cpp | 3 +-- src/ticks/fly_search.cpp | 13 +++++++------ src/ticks/mav_upload.cpp | 1 - src/ticks/path_gen.cpp | 4 ++-- 10 files changed, 26 insertions(+), 32 deletions(-) diff --git a/include/pathing/mission_path.hpp b/include/pathing/mission_path.hpp index 2d3bb7cc..f6ca21d6 100644 --- a/include/pathing/mission_path.hpp +++ b/include/pathing/mission_path.hpp @@ -1,11 +1,11 @@ #ifndef INCLUDE_PATHING_MISSION_PATH_HPP_ #define INCLUDE_PATHING_MISSION_PATH_HPP_ -#include - #include #include +#include + #include "utilities/datatypes.hpp" #include "protos/obc.pb.h" diff --git a/include/pathing/tree.hpp b/include/pathing/tree.hpp index 5dbde7db..f834877b 100644 --- a/include/pathing/tree.hpp +++ b/include/pathing/tree.hpp @@ -12,7 +12,6 @@ #include "utilities/datatypes.hpp" #include "utilities/obc_config.hpp" #include "utilities/rng.hpp" -#include "utilities/obc_config.hpp" class RRTNode; typedef std::vector RRTNodeList; diff --git a/include/ticks/tick.hpp b/include/ticks/tick.hpp index 5fc9cf53..966b09a1 100644 --- a/include/ticks/tick.hpp +++ b/include/ticks/tick.hpp @@ -34,10 +34,11 @@ class Tick { * because the constructor can possibly be called before actually transitioning into the tick, * as is the case for */ - virtual void init() {}; + virtual void init() {} constexpr TickID getID() const { return this->id; } constexpr const char* getName() const { return TICK_ID_TO_STR(this->id); } + protected: std::shared_ptr state; TickID id; diff --git a/src/pathing/mission_path.cpp b/src/pathing/mission_path.cpp index 9f9bf492..af8a8104 100644 --- a/src/pathing/mission_path.cpp +++ b/src/pathing/mission_path.cpp @@ -6,14 +6,14 @@ #include "utilities/logging.hpp" MissionPath::MissionPath(MissionPath::Type type, std::vector path, int hover_wait_s): - type(type), path(path), hover_wait_s(hover_wait_s) -{ + type(type), path(path), hover_wait_s(hover_wait_s) { switch (type) { case Type::HOVER: generateHoverCommands(); break; default: - LOG_F(WARNING, "Unknown MissionPath type %d, defaulting to forward", static_cast(type)); + LOG_F(WARNING, "Unknown MissionPath type %d, defaulting to forward", + static_cast(type)); case Type::FORWARD: generateForwardCommands(); break; @@ -84,7 +84,7 @@ void MissionPath::generateHoverCommands() { item.seq = i; item.frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; item.command = MAV_CMD_NAV_LOITER_TIME; - item.current = (i == 0) ? 1 : 0; // first waypoint should be true, others false + item.current = (i == 0) ? 1 : 0; // first waypoint should be true, others false item.autocontinue = 1; item.param1 = static_cast(this->hover_wait_s); item.param2 = 0.0f; // 0 => dont need to point heading at next waypoint diff --git a/src/pathing/static.cpp b/src/pathing/static.cpp index 1f44445a..d0c70633 100644 --- a/src/pathing/static.cpp +++ b/src/pathing/static.cpp @@ -17,7 +17,6 @@ #include "utilities/constants.hpp" #include "utilities/obc_config.hpp" #include "utilities/datatypes.hpp" -#include "utilities/obc_config.hpp" #include "utilities/rng.hpp" #include "utilities/common.hpp" @@ -405,10 +404,7 @@ std::vector ForwardCoveragePathing::generatePath( HoverCoveragePathing::HoverCoveragePathing(std::shared_ptr state): config{state->config.pathing.coverage}, drop_zone{state->mission_params.getAirdropBoundary()}, - state{state} -{ - -} + state{state} {} // Function to calculate the center of the polygon XYZCoord calculateCenter(const std::vector& polygon) { @@ -452,7 +448,8 @@ std::vector HoverCoveragePathing::run() { // right now just hardcoded to rectangles. think its ok to panic here because // we would want to stop early if we messed this up and this will happen // before takeoff - LOG_F(FATAL, "Hover airdrop pathing currently only supports 4 coordinates, not %lu", this->drop_zone.size()); + LOG_F(FATAL, "Hover airdrop pathing currently only supports 4 coordinates, not %lu", + this->drop_zone.size()); } XYZCoord top_left = this->drop_zone.at(3); @@ -473,9 +470,9 @@ std::vector HoverCoveragePathing::run() { Polygon scaled_drop_zone = this->drop_zone; scalePolygon(scaled_drop_zone, 1.20); - bool right = true; // start going from right to left + bool right = true; // start going from right to left for (double y = start_y; y > stop_y; y -= vision) { - std::vector row; // row of points either from left to right or right to left + std::vector row; // row of points either from left to right or right to left for (double x = start_x; x < stop_x; x += vision) { XYZCoord pt(x, y, altitude); if (Environment::isPointInPolygon(scaled_drop_zone, pt)) { @@ -483,7 +480,7 @@ std::vector HoverCoveragePathing::run() { } } if (!right) { - std::reverse(row.begin(), row.end()); + std::reverse(row.begin(), row.end()); } right = !right; hover_points.insert(std::end(hover_points), std::begin(row), std::end(row)); @@ -588,7 +585,7 @@ MissionPath generateSearchPath(std::shared_ptr state) { gps_coords.push_back(state->getCartesianConverter()->toLatLng(coord)); } - return MissionPath(MissionPath::Type::HOVER, gps_coords, + return MissionPath(MissionPath::Type::HOVER, gps_coords, state->config.pathing.coverage.hover.hover_time_s); } } diff --git a/src/ticks/active_takeoff.cpp b/src/ticks/active_takeoff.cpp index 2eb6ebc4..5147a1dc 100644 --- a/src/ticks/active_takeoff.cpp +++ b/src/ticks/active_takeoff.cpp @@ -9,9 +9,7 @@ #include "ticks/mission_prep.hpp" ActiveTakeoffTick::ActiveTakeoffTick(std::shared_ptr state): - Tick(state, TickID::ActiveTakeoff) -{ -} + Tick(state, TickID::ActiveTakeoff) {} std::chrono::milliseconds ActiveTakeoffTick::getWait() const { return ACTIVE_TAKEOFF_TICK_WAIT; diff --git a/src/ticks/cv_loiter.cpp b/src/ticks/cv_loiter.cpp index b3ab554d..aefbbb01 100644 --- a/src/ticks/cv_loiter.cpp +++ b/src/ticks/cv_loiter.cpp @@ -9,8 +9,7 @@ CVLoiterTick::CVLoiterTick(std::shared_ptr state): - Tick(state, TickID::CVLoiter) -{ + Tick(state, TickID::CVLoiter) { this->status = CVLoiterTick::Status::None; } diff --git a/src/ticks/fly_search.cpp b/src/ticks/fly_search.cpp index a71441a6..4780579f 100644 --- a/src/ticks/fly_search.cpp +++ b/src/ticks/fly_search.cpp @@ -11,10 +11,9 @@ using namespace std::chrono_literals; // NOLINT FlySearchTick::FlySearchTick(std::shared_ptr state): - Tick(state, TickID::FlySearch) -{ + Tick(state, TickID::FlySearch) { this->mission_started = false; - this->curr_mission_item = 1; // if this was 0 it would take a picture immediately after + this->curr_mission_item = 1; // if this was 0 it would take a picture immediately after // entering the search mission, so set to 1 so it doesn't start taking pictures until // actually over the search zone } @@ -47,7 +46,7 @@ Tick* FlySearchTick::tick() { // IMPORTANT: currently hardcoded to assume hover search pathing, so it // takes photos whenever it gets to a new waypoint (loiter position) - // if we were doing forward pathing would probably want to make it + // if we were doing forward pathing would probably want to make it // take photos at an interval but only when over the zone auto curr_waypoint = this->state->getMav()->curr_waypoint(); if (this->curr_mission_item != curr_waypoint) { @@ -55,8 +54,10 @@ Tick* FlySearchTick::tick() { auto photo = this->state->getCamera()->takePicture(500ms, this->state->getMav()); if (photo.has_value()) { - this->last_photo_time = getUnixTime_ms(); // Update the last photo time - this->state->getCV()->runPipeline(photo.value()); // Run the pipeline on the photo + // Update the last photo time + this->last_photo_time = getUnixTime_ms(); + // Run the pipeline on the photo + this->state->getCV()->runPipeline(photo.value()); } } this->curr_mission_item = curr_waypoint; diff --git a/src/ticks/mav_upload.cpp b/src/ticks/mav_upload.cpp index bb04a113..24e18518 100644 --- a/src/ticks/mav_upload.cpp +++ b/src/ticks/mav_upload.cpp @@ -17,7 +17,6 @@ MavUploadTick::MavUploadTick(std::shared_ptr state, Tick(state, TickID::MavUpload), next_tick{next_tick}, waypoints{waypoints}, upload_geofence{upload_geofence} { - } std::chrono::milliseconds MavUploadTick::getWait() const { diff --git a/src/ticks/path_gen.cpp b/src/ticks/path_gen.cpp index 02cca714..fe690df0 100644 --- a/src/ticks/path_gen.cpp +++ b/src/ticks/path_gen.cpp @@ -11,7 +11,7 @@ #include "ticks/path_validate.hpp" #include "utilities/logging.hpp" -using namespace std::chrono_literals; +using namespace std::chrono_literals; // NOLINT PathGenTick::PathGenTick(std::shared_ptr state) : Tick(state, TickID::PathGen) {} @@ -38,4 +38,4 @@ 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); -} \ No newline at end of file +} From d034a96c5e730e9b0ba3ae5d46d3815cfabd5b19 Mon Sep 17 00:00:00 2001 From: Tyler Lentz Date: Wed, 12 Jun 2024 04:19:27 +0000 Subject: [PATCH 11/13] load in mavlink parameters via config file --- configs/config.json | 3 +++ configs/dev-config.json | 3 +++ include/network/mavlink.hpp | 3 ++- include/utilities/obc_config.hpp | 6 ++++++ src/core/obc.cpp | 2 +- src/network/mavlink.cpp | 29 ++++++++++++++++------------- src/utilities/obc_config.cpp | 4 ++++ 7 files changed, 35 insertions(+), 15 deletions(-) diff --git a/configs/config.json b/configs/config.json index 1bf1b28e..f476acac 100644 --- a/configs/config.json +++ b/configs/config.json @@ -83,5 +83,8 @@ "gain_auto_upper_limit": 10, "gain_auto_lower_limit": 1 } + }, + "mavlink_parameters": { + "Q_GUIDED_MODE": 1 } } \ No newline at end of file diff --git a/configs/dev-config.json b/configs/dev-config.json index 717b78a4..025c728c 100644 --- a/configs/dev-config.json +++ b/configs/dev-config.json @@ -83,5 +83,8 @@ "gain_auto_upper_limit": 10, "gain_auto_lower_limit": 1 } + }, + "mavlink_parameters": { + "Q_GUIDED_MODE": 1 } } \ No newline at end of file diff --git a/include/network/mavlink.hpp b/include/network/mavlink.hpp index b718c594..02f4e7c2 100644 --- a/include/network/mavlink.hpp +++ b/include/network/mavlink.hpp @@ -22,6 +22,7 @@ #include "protos/obc.pb.h" #include "utilities/datatypes.hpp" #include "pathing/mission_path.hpp" +#include "utilities/obc_config.hpp" class MissionState; @@ -33,7 +34,7 @@ class MavlinkClient { * example: * MavlinkClient("tcp://192.168.65.254:5762") */ - explicit MavlinkClient(std::string link); + explicit MavlinkClient(OBCConfig config); /* * BLOCKING. Continues to try to upload the mission based on the passed through MissionConfig diff --git a/include/utilities/obc_config.hpp b/include/utilities/obc_config.hpp index d7e4a5cb..36906a61 100644 --- a/include/utilities/obc_config.hpp +++ b/include/utilities/obc_config.hpp @@ -5,6 +5,7 @@ #include #include #include +#include #include #include "udp_squared/internal/enum.h" #include "utilities/constants.hpp" @@ -156,6 +157,10 @@ struct CameraConfig { } lucid; }; +struct MavlinkParametersConfig { + std::unordered_map param_map; +}; + struct OBCConfig { LoggingConfig logging; NetworkConfig network; @@ -163,6 +168,7 @@ struct OBCConfig { CVConfig cv; PathingConfig pathing; CameraConfig camera; + MavlinkParametersConfig mavlink_parameters; // Load user specified config json, or make a new one OBCConfig(int argc, char* argv[]); diff --git a/src/core/obc.cpp b/src/core/obc.cpp index b04110c2..e46891ab 100644 --- a/src/core/obc.cpp +++ b/src/core/obc.cpp @@ -60,7 +60,7 @@ void OBC::connectMavlink(std::string mavlink_url) { loguru::set_thread_name("mav connect"); // TODO: pull mav ip from config file - std::shared_ptr mav(new MavlinkClient(mavlink_url)); + std::shared_ptr mav(new MavlinkClient(this->state->config)); this->state->setMav(mav); } diff --git a/src/network/mavlink.cpp b/src/network/mavlink.cpp index ae7a3e6e..1438de0a 100644 --- a/src/network/mavlink.cpp +++ b/src/network/mavlink.cpp @@ -21,11 +21,14 @@ #include "pathing/mission_path.hpp" #include "utilities/locks.hpp" #include "utilities/logging.hpp" +#include "utilities/obc_config.hpp" using namespace std::chrono_literals; // NOLINT -MavlinkClient::MavlinkClient(std::string link) +MavlinkClient::MavlinkClient(OBCConfig config) : mavsdk(mavsdk::Mavsdk::Configuration(mavsdk::Mavsdk::ComponentType::CompanionComputer)) { + std::string link = config.network.mavlink.connect; + LOG_F(INFO, "Connecting to Mav at %s", link.c_str()); while (true) { @@ -63,19 +66,19 @@ MavlinkClient::MavlinkClient(std::string link) this->passthrough = std::make_unique(system); this->param = std::make_unique(system); - // need to make sure this parameter is enabled so that during the search phase - // it stops to loiter at the loiter points and doesn't just stay in fixed wing mode - // the whole time - LOG_F(INFO, "Setting Q_GUIDED_MODE to 1..."); - while (true) { - auto result = this->param->set_param_int("Q_GUIDED_MODE", 1); - if (result != mavsdk::Param::Result::Success) { - LOG_S(ERROR) << "Failed to set Q_GUIDED_MODE: " << result; - std::this_thread::sleep_for(1s); - continue; + // iterate through all config parameters and upload to the plane + for (const auto& [param, val] : config.mavlink_parameters.param_map) { + LOG_F(INFO, "Setting %s to %d", param.c_str(), val); + while (true) { + auto result = this->param->set_param_int(param, val); + if (result != mavsdk::Param::Result::Success) { + LOG_S(ERROR) << "Failed to set param " << result; + std::this_thread::sleep_for(1s); + continue; + } + LOG_F(INFO, "Successfully set param"); + break; } - LOG_F(INFO, "Set Q_GUIDED_MODE to %d", 1); - break; } LOG_F(INFO, "Logging out all mavlink params at TRACE level..."); diff --git a/src/utilities/obc_config.cpp b/src/utilities/obc_config.cpp index a046015e..42576e0a 100644 --- a/src/utilities/obc_config.cpp +++ b/src/utilities/obc_config.cpp @@ -95,4 +95,8 @@ OBCConfig::OBCConfig(int argc, char* argv[]) { SET_CONFIG_OPT(camera, lucid, gain_auto_lower_limit); SET_CONFIG_OPT(takeoff, altitude_m); + + for (const auto& [param, val] : configs.at("mavlink_parameters").items()) { + this->mavlink_parameters.param_map.insert({param, val}); + } } From 118ba373c7656d7ee16edc4c8cf5d262b92374f4 Mon Sep 17 00:00:00 2001 From: Tyler Lentz Date: Wed, 12 Jun 2024 04:27:07 +0000 Subject: [PATCH 12/13] json with comments :) --- configs/config.json | 2 +- configs/dev-config.json | 2 +- src/utilities/obc_config.cpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/configs/config.json b/configs/config.json index f476acac..9877dafc 100644 --- a/configs/config.json +++ b/configs/config.json @@ -85,6 +85,6 @@ } }, "mavlink_parameters": { - "Q_GUIDED_MODE": 1 + "Q_GUIDED_MODE": 1 // hover over loiter waypoints } } \ No newline at end of file diff --git a/configs/dev-config.json b/configs/dev-config.json index 025c728c..096ba40b 100644 --- a/configs/dev-config.json +++ b/configs/dev-config.json @@ -85,6 +85,6 @@ } }, "mavlink_parameters": { - "Q_GUIDED_MODE": 1 + "Q_GUIDED_MODE": 1 // hover over loiter waypoints } } \ No newline at end of file diff --git a/src/utilities/obc_config.cpp b/src/utilities/obc_config.cpp index 42576e0a..d8fc31ab 100644 --- a/src/utilities/obc_config.cpp +++ b/src/utilities/obc_config.cpp @@ -26,7 +26,7 @@ OBCConfig::OBCConfig(int argc, char* argv[]) { } // 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); + nlohmann::json configs = nlohmann::json::parse(configStream, nullptr, true, true); // Read this in first before anything else so that all of the read in values get logged // to the config file. Otherwise they will be output to the terminal but not saved to From f53a9cb39a74d8c8111b5e0db53e40193e1c7d24 Mon Sep 17 00:00:00 2001 From: Tyler Lentz Date: Wed, 12 Jun 2024 04:38:50 +0000 Subject: [PATCH 13/13] fix merge conflict in integration test --- tests/integration/path_planning.cpp | 7 +------ 1 file changed, 1 insertion(+), 6 deletions(-) diff --git a/tests/integration/path_planning.cpp b/tests/integration/path_planning.cpp index 4aa0b27d..98d308dd 100644 --- a/tests/integration/path_planning.cpp +++ b/tests/integration/path_planning.cpp @@ -283,11 +283,6 @@ int main() { // RRT settings (manually put in) double search_radius = 9999; -<<<<<<< HEAD - double rewire_radius = 256; - RRTConfig config = RRTConfig { num_iterations, rewire_radius, true, PointFetchMethod::Enum::NEAREST, true }; -======= ->>>>>>> main RRT rrt = RRT(start, goals, search_radius, state->mission_params.getFlightBoundary(), state->config, obstacles, {}); @@ -305,7 +300,7 @@ int main() { // get the path, put it into the file std::vector path = rrt.getPointsToGoal(); std::cout << "Path size: " << path.size() << std::endl; - std::cout << "Path length: " << (path.size() * POINT_SEPARATION) << std::endl; + std::cout << "Path length: " << (path.size() * state->config.pathing.dubins.point_separation) << std::endl; for (const XYZCoord& point : path) { file << point.z << std::endl; }