From a089062cbfd2063cd9e287dee76e7b5aa699d0f4 Mon Sep 17 00:00:00 2001 From: Askew <87111319+AskewParity@users.noreply.github.com> Date: Mon, 8 Apr 2024 12:08:31 -0700 Subject: [PATCH] Feat/pathing airdrop (#142) * Infrastructure + Naive Search Method (not bounds safe) * Add integration test, change Environment to reflect new information * Unit Testing (not finished, but 'should' work), refactor header-->source * fix broken include because of file rename --------- Co-authored-by: Tyler Lentz --- include/pathing/environment.hpp | 61 ++++-- include/pathing/static.hpp | 29 +++ include/utilities/constants.hpp | 24 +-- src/pathing/environment.cpp | 114 ++++++++--- src/pathing/static.cpp | 31 ++- tests/integration/CMakeLists.txt | 16 ++ tests/integration/airdrop_pathing.cpp | 274 ++++++++++++++++++++++++++ tests/unit/environment_test.cpp | 77 ++++++-- tests/unit/pathing/tree_test.cpp | 4 +- tests/unit/pathing_test.cpp | 2 +- 10 files changed, 559 insertions(+), 73 deletions(-) create mode 100644 tests/integration/airdrop_pathing.cpp diff --git a/include/pathing/environment.hpp b/include/pathing/environment.hpp index 611429a2..6316552d 100644 --- a/include/pathing/environment.hpp +++ b/include/pathing/environment.hpp @@ -21,8 +21,8 @@ */ class Environment { public: - Environment(const Polygon& valid_region, const std::vector& goals, - const std::vector& obstacles); + Environment(const Polygon& valid_region, const Polygon& airdrop_zone, + const std::vector& goals, const std::vector& obstacles); /** * Check if a point is in the valid region @@ -123,16 +123,6 @@ class Environment { */ bool isLineInBounds(const XYZCoord& start_point, const XYZCoord& end_point) const; - /** - * Find the bounds of the valid region (i.e. the max/min x and y values). - * - * ASSUMES valid_region has already been created - * - * @return a pair of pairs, where the first pair is min/max x values and the second is the - * min/max y values - */ - std::pair, std::pair> findBounds() const; - /** * Determines whether a line segment intersects the polygon * @@ -172,8 +162,44 @@ class Environment { */ bool intersect(XYZCoord p1, XYZCoord q1, XYZCoord p2, XYZCoord q2) const; + /** + * Returns endpoints (of vertical lines) on airdrop_zone for coverage pathing + * + * TODO - UNIT TESTS + * + * @return the endpoints on the airdrop zone + */ + std::vector getAirdropEndpoints(int scan_radius) const; + + /** + * Fills an intersection if one exists between an edge of the polygon and the VERTICAL ray + * + * @param p1 the first point of the edge + * @param p2 the second point of the edge + * @param rayStart the start of the ray + * @param rayEnd the end of the ray + * @param intersection the intersection point (WILL BE FILLED IF INTERSECTION EXISTS) + * @return true if an intersection exists, false otherwise + */ + bool rayIntersectsEdge(const XYZCoord& p1, const XYZCoord& p2, const XYZCoord& rayStart, + const XYZCoord& rayEnd, XYZCoord& intersection) const; + + /** + * Finds all intersections between a VERTICAL ray and a polygon, and returns them as a lit + * + * TODO - UNIT TESTS + * + * @param polygon the polygon to check intersections + * @param rayStart the start of the ray + * @param rayEnd the end of the ray + * @return a list of intersections + */ + std::vector findIntersections(const Polygon& polygon, const XYZCoord& rayStart, + const XYZCoord& rayEnd) const; + private: const Polygon valid_region; // boundary of the valid map + const Polygon airdrop_zone; // boundary of the airdrop zone (subset of valid_region) const std::vector goals; // goal point const std::vector obstacles; // obstacles in the map @@ -186,6 +212,17 @@ class Environment { // is (min x, max x), // second pair is // (min y, max y) + + /** + * Find the bounds of the valid region (i.e. the max/min x and y values). + * + * ASSUMES valid_region has already been created + * + * @return a pair of pairs, where the first pair is min/max x values and the second is the + * min/max y values + */ + std::pair, std::pair> findBounds( + const Polygon& bounds) const; }; #endif // INCLUDE_PATHING_ENVIRONMENT_HPP_ diff --git a/include/pathing/static.hpp b/include/pathing/static.hpp index cd93ce76..6f011d75 100644 --- a/include/pathing/static.hpp +++ b/include/pathing/static.hpp @@ -161,6 +161,35 @@ class RRT { void optimizeTree(RRTNode *sample); }; +/** + * Class that performs Coverage-Path_Planning (CPP) over a given polygon + * + * Basically draws vertical lines, and the connects them with Dubins paths + * + * Limitations + * - Cannot path through non-convex shapes + * - Does not check if path is inbounds or not + */ +class AirdropSearch { + public: + AirdropSearch(const RRTPoint &start, double scan_radius, Polygon bounds, Polygon airdrop_zone, + std::vector obstacles = {}); + + /** + * Generates a path of parallel lines to cover a given area + * + * @return ==> list of 2-vectors describing the path through the aridrop_zone + */ + std::vector run() const; + + private: + const double scan_radius; // how far each side of the plane we intend to look (half dist + // between search lines) + 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 +}; + std::vector generateInitialPath(std::shared_ptr state); #endif // INCLUDE_PATHING_STATIC_HPP_ diff --git a/include/utilities/constants.hpp b/include/utilities/constants.hpp index 9559394f..0a29b844 100644 --- a/include/utilities/constants.hpp +++ b/include/utilities/constants.hpp @@ -15,24 +15,26 @@ const double TWO_PI = 2 * M_PI; const double HALF_PI = M_PI / 2; // FROM OBC PYTHON -const double TURNING_RADIUS = 30; -const double POINT_SEPARATION = 10; +const double TURNING_RADIUS = 30.0; +const double POINT_SEPARATION = 10.0; // RRT CONSTANTS -const int ITERATIONS_PER_WAYPOINT = 200; -const double SEARCH_RADIUS = 1000; -const double REWIRE_RADIUS = 200; +const int ITERATIONS_PER_WAYPOINT = 200; // number of times RRT is ran per waypoint +const double SEARCH_RADIUS = + 1000.0; // DOES NOTHING, limits how far off the tree the new node can be +const double REWIRE_RADIUS = 200.0; // ONLY FOR RRT-STAR, max radius from new node to rewire // RRT HELPER CONSTANTS -const double EPOCH_TEST_MARGIN = 0.97; -const int ENV_PATH_VALIDATION_STEP_SIZE = 5; -const int K_RANDOM_NODES = 100; -const int K_CLOESEST_NODES = 50; +const double EPOCH_TEST_MARGIN = 0.97; // at what margin of improvement does it stop +const int ENV_PATH_VALIDATION_STEP_SIZE = 5; // how many points to skip when validating path +const int NUM_EPOCHS = 5; // number of times to evaulate the path length +const int K_RANDOM_NODES = 100; // how many nodes to generate for the tree +const int K_CLOESEST_NODES = 50; // how many nodes to look at when finding the closest node const int TOTAL_OPTIONS_FOR_GOAL_CONNECTION = 2048; // TODO - MUST SCALE WITH ITERATIONS OR ELSE CANT FIND GOAL -const int TRIES_FOR_RANDOM_POINT = 64; // for generating random points -const int MAX_DUBINS_OPTIONS_TO_PARSE = 16; +const int TRIES_FOR_RANDOM_POINT = 64; // for generating random points +const int MAX_DUBINS_OPTIONS_TO_PARSE = 16; // how many routes to check when connecting two nodes const int DEFAULT_GCS_PORT = 5010; diff --git a/src/pathing/environment.cpp b/src/pathing/environment.cpp index 6f45ea7d..1f1004fe 100644 --- a/src/pathing/environment.cpp +++ b/src/pathing/environment.cpp @@ -10,12 +10,13 @@ #include "utilities/datatypes.hpp" #include "utilities/rng.hpp" -Environment::Environment(const Polygon& valid_region, const std::vector& goals, - const std::vector& obstacles) +Environment::Environment(const Polygon& valid_region, const Polygon& airdrop_zone, + const std::vector& goals, const std::vector& obstacles) : valid_region(valid_region), + airdrop_zone(airdrop_zone), goals(goals), goals_found(0), - bounds(findBounds()), + bounds(findBounds(valid_region)), obstacles(obstacles) {} bool Environment::isPointInBounds(const XYZCoord& point) const { @@ -165,26 +166,6 @@ bool Environment::isLineInBounds(const XYZCoord& start_point, const XYZCoord& en return true; } -std::pair, std::pair> Environment::findBounds() const { - if (valid_region.empty()) { - return std::make_pair(std::make_pair(0, 0), std::make_pair(0, 0)); - } - - double min_x = valid_region[0].x; - double max_x = valid_region[0].x; - double min_y = valid_region[0].y; - double max_y = valid_region[0].y; - - for (const XYZCoord& point : valid_region) { - min_x = std::min(min_x, point.x); - max_x = std::max(max_x, point.x); - min_y = std::min(min_y, point.y); - max_y = std::max(max_y, point.y); - } - - return {{min_x, max_x}, {min_y, max_y}}; -} - bool Environment::doesLineIntersectPolygon(const XYZCoord& start_point, const XYZCoord& end_point, const Polygon& polygon) const { for (int i = 0, j = polygon.size() - 1; i < polygon.size(); j = i++) { @@ -243,3 +224,90 @@ bool Environment::intersect(XYZCoord p1, XYZCoord q1, XYZCoord p2, XYZCoord q2) return false; // Doesn't fall in any of the above cases } + +std::vector Environment::getAirdropEndpoints(int scan_radius) const { + auto bounds = findBounds(airdrop_zone); + auto [x_min, x_max] = bounds.first; + auto [y_min, y_max] = bounds.second; + + std::vector endpoints; + bool fly_down = true; + for (double x = x_min + scan_radius; x <= x_max - scan_radius; x += scan_radius * 2) { + // finds where this x-coordinate intersects with the airdrop zone (always convex) + const XYZCoord top(x, y_max, 0); + const XYZCoord bottom(x, y_min, 0); + std::vector intersections = findIntersections(airdrop_zone, top, bottom); + + // adjacent lines should be in different directions + double angle = fly_down ? 3 * M_PI / 2 : HALF_PI; + RRTPoint top_vector(intersections[0], angle); + RRTPoint bottom_vector(intersections[1], angle); + + if (fly_down) { + endpoints.push_back(top_vector); + endpoints.push_back(bottom_vector); + } else { + endpoints.push_back(bottom_vector); + endpoints.push_back(top_vector); + } + + fly_down = !fly_down; + } + + return endpoints; +} + +bool Environment::rayIntersectsEdge(const XYZCoord& p1, const XYZCoord& p2, + const XYZCoord& rayStart, const XYZCoord& rayEnd, + XYZCoord& intersection) const { + // if the x coordinate lines between the edge + if ((p2.x <= rayStart.x && p1.x >= rayStart.x) || (p1.x <= rayStart.x && p2.x >= rayStart.x)) { + double slope = (p2.y - p1.y) / (p2.x - p1.x); + // finds where they intersect using a line y - y' = m(x - x') + intersection = XYZCoord(rayStart.x, slope * (rayStart.x - p1.x) + p1.y, 0); + return true; + } + return false; +} + +std::vector Environment::findIntersections(const Polygon& polygon, + const XYZCoord& rayStart, + const XYZCoord& rayEnd) const { + // array to be filled + std::vector intersections; + int n = polygon.size(); + + for (int i = 0; i < n; ++i) { + const XYZCoord& p1 = polygon[i]; + const XYZCoord& p2 = polygon[(i + 1) % n]; + + // temporary variable to be changed if an intersection is found + XYZCoord intersection(0, 0, 0); + if (rayIntersectsEdge(p1, p2, rayStart, rayEnd, intersection)) { + intersections.push_back(intersection); + } + } + + return intersections; +} + +std::pair, std::pair> Environment::findBounds( + const Polygon& region) const { + if (region.empty()) { + return std::make_pair(std::make_pair(0, 0), std::make_pair(0, 0)); + } + + double min_x = region[0].x; + double max_x = region[0].x; + double min_y = region[0].y; + double max_y = region[0].y; + + for (const XYZCoord& point : region) { + min_x = std::min(min_x, point.x); + max_x = std::max(max_x, point.x); + min_y = std::min(min_y, point.y); + max_y = std::max(max_y, point.y); + } + + return {{min_x, max_x}, {min_y, max_y}}; +} diff --git a/src/pathing/static.cpp b/src/pathing/static.cpp index ce7a614f..9129c643 100644 --- a/src/pathing/static.cpp +++ b/src/pathing/static.cpp @@ -24,7 +24,8 @@ RRT::RRT(RRTPoint start, std::vector goals, int iterations_per_waypoin : iterations_per_waypoint(iterations_per_waypoint), search_radius(search_radius), rewire_radius(rewire_radius), - tree(start, Environment(bounds, goals, obstacles), Dubins(TURNING_RADIUS, POINT_SEPARATION)), + tree(start, Environment(bounds, {}, goals, obstacles), + Dubins(TURNING_RADIUS, POINT_SEPARATION)), config(config) {} void RRT::run() { @@ -54,7 +55,7 @@ void RRT::run() { std::vector RRT::getPointsToGoal() const { return tree.getPathToGoal(); } bool RRT::RRTIteration(int tries, int current_goal_index) { - const int epoch_interval = tries / 5; + const int epoch_interval = tries / NUM_EPOCHS; int current_epoch = epoch_interval; RRTNode *goal_node = nullptr; @@ -244,8 +245,7 @@ std::vector generateInitialPath(std::shared_ptr state) { // the other waypoitns is the goals if (state->config.getWaypoints().size() < 2) { loguru::set_thread_name("Static Pathing"); - LOG_F(ERROR, - "Not enough waypoints to generate a path, required 2+, existing waypoints: %s", + LOG_F(ERROR, "Not enough waypoints to generate a path, required 2+, existing waypoints: %s", std::to_string(state->config.getWaypoints().size()).c_str()); return {}; } @@ -273,3 +273,26 @@ std::vector generateInitialPath(std::shared_ptr state) { return output_coords; } + +AirdropSearch::AirdropSearch(const RRTPoint &start, double scan_radius, Polygon bounds, + Polygon airdrop_zone, std::vector obstacles) + : start(start), + scan_radius(scan_radius), + airspace(Environment(bounds, airdrop_zone, {}, obstacles)), + dubins(Dubins(std::min(scan_radius, TURNING_RADIUS), POINT_SEPARATION)) {} + +std::vector AirdropSearch::run() const { + // generates the endpoints for the lines (including headings) + std::vector waypoints = airspace.getAirdropEndpoints(scan_radius); + + // generates the path connecting the q + std::vector path; + RRTPoint current = start; + for (auto &waypoint : waypoints) { + std::vector dubins_path = dubins.dubinsPath(current, waypoint); + path.insert(path.end(), dubins_path.begin() + 1, dubins_path.end()); + current = waypoint; + } + + return path; +} diff --git a/tests/integration/CMakeLists.txt b/tests/integration/CMakeLists.txt index da047080..3e9d1d48 100644 --- a/tests/integration/CMakeLists.txt +++ b/tests/integration/CMakeLists.txt @@ -146,6 +146,22 @@ target_add_loguru(path_planning) target_include_directories(path_planning PRIVATE ${ImageMagick_INCLUDE_DIRS}) target_link_libraries(path_planning PRIVATE -Wl,--copy-dt-needed-entries ${ImageMagick_LIBRARIES}) +add_executable(airdrop_pathing "airdrop_pathing.cpp") +target_link_libraries(airdrop_pathing PRIVATE obcpp_lib) +target_include_directories(airdrop_pathing PRIVATE ${INCLUDE_DIRECTORY}) +target_add_protobuf(airdrop_pathing) +target_add_torch(airdrop_pathing) +target_add_torchvision(airdrop_pathing) +target_add_json(airdrop_pathing) +target_add_opencv(airdrop_pathing) +target_add_httplib(airdrop_pathing) +target_add_mavsdk(airdrop_pathing) +target_add_matplot(airdrop_pathing) +target_add_loguru(airdrop_pathing) + +target_include_directories(airdrop_pathing PRIVATE ${ImageMagick_INCLUDE_DIRS}) +target_link_libraries(airdrop_pathing PRIVATE -Wl,--copy-dt-needed-entries ${ImageMagick_LIBRARIES}) + # airdrop_sockets # add_executable(airdrop_sockets src/network/airdrop_sockets.c tests/integration/airdrop_sockets.c) # target_include_directories(airdrop_sockets PRIVATE ${INCLUDE_DIRECTORY}) diff --git a/tests/integration/airdrop_pathing.cpp b/tests/integration/airdrop_pathing.cpp new file mode 100644 index 00000000..fee47fc1 --- /dev/null +++ b/tests/integration/airdrop_pathing.cpp @@ -0,0 +1,274 @@ +#include +#include +#include +#include +#include +#include +#include + +#include "core/mission_state.hpp" +#include "network/gcs.hpp" +#include "network/gcs_macros.hpp" +#include "network/gcs_routes.hpp" +#include "pathing/plotting.hpp" +#include "pathing/static.hpp" +#include "ticks/mission_prep.hpp" +#include "ticks/mav_upload.hpp" +#include "ticks/path_gen.hpp" +#include "ticks/tick.hpp" +#include "utilities/constants.hpp" +#include "utilities/datatypes.hpp" +#include "utilities/http.hpp" + +#define DECLARE_HANDLER_PARAMS(STATE, REQ, RESP) \ + std::shared_ptr STATE = std::make_shared(); \ + httplib::Request REQ; \ + httplib::Response RESP + +const static char* mission_json_2020 = R"( +{ + "BottleAssignments": [ + { + "Alphanumeric": "", + "AlphanumericColor": 0, + "Shape": 0, + "ShapeColor": 0, + "Index": 1, + "IsMannikin": true + }, + { + "Alphanumeric": "", + "AlphanumericColor": 0, + "Shape": 0, + "ShapeColor": 0, + "Index": 2, + "IsMannikin": true + }, + { + "Alphanumeric": "", + "AlphanumericColor": 0, + "Shape": 0, + "ShapeColor": 0, + "Index": 3, + "IsMannikin": true + }, + { + "Alphanumeric": "", + "AlphanumericColor": 0, + "Shape": 0, + "ShapeColor": 0, + "Index": 4, + "IsMannikin": true + }, + { + "Alphanumeric": "", + "AlphanumericColor": 0, + "Shape": 0, + "ShapeColor": 0, + "Index": 5, + "IsMannikin": true + } + ], + "FlightBoundary": [ + { + "Latitude": 38.1462694444444, + "Longitude": -76.4281638888889 + }, + { + "Latitude": 38.151625, + "Longitude": -76.4286833333333 + }, + { + "Latitude": 38.1518888888889, + "Longitude": -76.4314666666667 + }, + { + "Latitude": 38.1505944444444, + "Longitude": -76.4353611111111 + }, + { + "Latitude": 38.1475666666667, + "Longitude": -76.4323416666667 + }, + { + "Latitude": 38.1446666666667, + "Longitude": -76.4329472222222 + }, + { + "Latitude": 38.1432555555556, + "Longitude": -76.4347666666667 + }, + { + "Latitude": 38.1404638888889, + "Longitude": -76.4326361111111 + }, + { + "Latitude": 38.1407194444444, + "Longitude": -76.4260138888889 + }, + { + "Latitude": 38.1437611111111, + "Longitude": -76.4212055555556 + }, + { + "Latitude": 38.1473472222222, + "Longitude": -76.4232111111111 + }, + { + "Latitude": 38.1461305555556, + "Longitude": -76.4266527777778 + } + ], + "AirdropBoundary": [ + { + "Latitude": 38.1444444444444, + "Longitude": -76.4280916666667 + }, + { + "Latitude": 38.1459444444444, + "Longitude": -76.4237944444445 + }, + { + "Latitude": 38.1439305555556, + "Longitude": -76.4227444444444 + }, + { + "Latitude": 38.1417138888889, + "Longitude": -76.4253805555556 + }, + { + "Latitude": 38.1412111111111, + "Longitude": -76.4322361111111 + }, + { + "Latitude": 38.1431055555556, + "Longitude": -76.4335972222222 + }, + { + "Latitude": 38.1441805555556, + "Longitude": -76.4320111111111 + }, + { + "Latitude": 38.1452611111111, + "Longitude": -76.4289194444444 + }, + { + "Latitude": 38.1444444444444, + "Longitude": -76.4280916666667 + } + ], + "Waypoints": [ + { + "Latitude": 38.1446916666667, + "Longitude": -76.4279944444445, + "Altitude": 200.0 + }, + { + "Latitude": 38.1461944444444, + "Longitude": -76.4237138888889, + "Altitude": 300.0 + }, + { + "Latitude": 38.1438972222222, + "Longitude": -76.42255, + "Altitude": 400.0 + }, + { + "Latitude": 38.1417722222222, + "Longitude": -76.4251083333333, + "Altitude": 400.0 + }, + { + "Latitude": 38.14535, + "Longitude": -76.428675, + "Altitude": 300.0 + }, + { + "Latitude": 38.1508972222222, + "Longitude": -76.4292972222222, + "Altitude": 300.0 + }, + { + "Latitude": 38.1514944444444, + "Longitude": -76.4313833333333, + "Altitude": 300.0 + }, + { + "Latitude": 38.1505333333333, + "Longitude": -76.434175, + "Altitude": 300.0 + }, + { + "Latitude": 38.1479472222222, + "Longitude": -76.4316055555556, + "Altitude": 200.0 + }, + { + "Latitude": 38.1443333333333, + "Longitude": -76.4322888888889, + "Altitude": 200.0 + }, + { + "Latitude": 38.1433166666667, + "Longitude": -76.4337111111111, + "Altitude": 300.0 + }, + { + "Latitude": 38.1410944444444, + "Longitude": -76.4321555555556, + "Altitude": 400.0 + }, + { + "Latitude": 38.1415777777778, + "Longitude": -76.4252472222222, + "Altitude": 400.0 + }, + { + "Latitude": 38.1446083333333, + "Longitude": -76.4282527777778, + "Altitude": 200.0 + } + ] +})"; + +/* + * FILE OUTPUT LOCATIONS + * |- build + * |- pathing_output + * |- test_airdrop_pathing.jpg + * |- test_airdrop_pathing.gif (if enabled) + * |- airdop_search_coords.txt + * + * This rough integration test is to test the airdrop search pathing algorithm + */ +int main() { + std::cout << "Messing with Airdrop Zone Search Pathing" << std::endl; + // First upload a mission so that we generate a path + // this is roughly the mission from 2020 + DECLARE_HANDLER_PARAMS(state, req, resp); + req.body = mission_json_2020; + state->setTick(new MissionPrepTick(state)); + + GCS_HANDLE(Post, mission)(state, req, resp); + + // files to put path_coordinates to + std::ofstream file; + file.open("airdop_search_coords.txt"); + + RRTPoint start = RRTPoint(state->config.getWaypoints()[0], 0); + + AirdropSearch search(start, 20, state->config.getFlightBoundary(), + state->config.getAirdropBoundary()); + + std::vector path = search.run(); + + // plot the path + std::cout << "Start Plotting" << std::endl; + PathingPlot plotter("pathing_output", state->config.getFlightBoundary(), + state->config.getAirdropBoundary(), {}); + + plotter.addFinalPolyline(path); + plotter.output("test_airdrop_pathing", PathOutputType::STATIC); + file.close(); + return 0; +} \ No newline at end of file diff --git a/tests/unit/environment_test.cpp b/tests/unit/environment_test.cpp index be5c67a3..1df44c3f 100644 --- a/tests/unit/environment_test.cpp +++ b/tests/unit/environment_test.cpp @@ -5,7 +5,6 @@ #include "utilities/constants.hpp" #include "utilities/datatypes.hpp" - /* * tests Polygon::pointInBounds */ @@ -15,7 +14,7 @@ TEST(EnvironmentTest, PointInBounds) { test.emplace_back(XYZCoord{0, 1, 0}); test.emplace_back(XYZCoord{0, 0, 0}); test.emplace_back(XYZCoord{1, 0, 0}); - Environment test_env = {test, {XYZCoord(0, 0, 0)}, {}}; + Environment test_env = {test, {}, {XYZCoord(0, 0, 0)}, {}}; EXPECT_EQ(true, test_env.isPointInPolygon(test, XYZCoord{0.5, 0.5, 0})); EXPECT_EQ(true, test_env.isPointInPolygon(test, XYZCoord{0.5, 0.5, 99999999})); @@ -27,7 +26,7 @@ TEST(EnvironmentTest, PointInBounds) { EXPECT_EQ(false, test_env.isPointInPolygon(test, XYZCoord{0.5, -1, 0})); // down Polygon no_point = {}; - Environment no_point_env = {no_point, {XYZCoord(0, 0, 0)}, {}}; + Environment no_point_env = {no_point, {}, {XYZCoord(0, 0, 0)}, {}}; EXPECT_EQ(false, no_point_env.isPointInPolygon(no_point, XYZCoord{1, 1, 1})); EXPECT_EQ(false, no_point_env.isPointInPolygon(no_point, XYZCoord{1, 0, 1})); @@ -35,7 +34,7 @@ TEST(EnvironmentTest, PointInBounds) { Polygon point; point.emplace_back(XYZCoord{1, 1, 1}); - Environment point_env = {point, {XYZCoord(0, 0, 0)}, {}}; + Environment point_env = {point, {}, {XYZCoord(0, 0, 0)}, {}}; EXPECT_EQ(false, point_env.isPointInPolygon(point, XYZCoord{1, 1, 1})); EXPECT_EQ(false, point_env.isPointInPolygon(point, XYZCoord{1, 0, 1})); @@ -47,7 +46,7 @@ TEST(EnvironmentTest, PointInBounds) { quadrilateral.emplace_back(XYZCoord{2, 1, 0}); quadrilateral.emplace_back(XYZCoord{4, 4, 0}); quadrilateral.emplace_back(XYZCoord{1, 2, 0}); - Environment quadrilateral_env = {quadrilateral, {XYZCoord(0, 0, 0)}, {}}; + Environment quadrilateral_env = {quadrilateral, {}, {XYZCoord(0, 0, 0)}, {}}; EXPECT_EQ(true, quadrilateral_env.isPointInPolygon(quadrilateral, XYZCoord{1.5, 1.00, 0})); EXPECT_EQ(true, quadrilateral_env.isPointInPolygon(quadrilateral, XYZCoord{0.5, 0.90, 0})); @@ -60,7 +59,6 @@ TEST(EnvironmentTest, PointInBounds) { EXPECT_EQ(false, quadrilateral_env.isPointInPolygon(quadrilateral, XYZCoord{1.5, 2.50, 0})); } - /* * Tests Environment::isPointInBounds * @@ -78,7 +76,7 @@ TEST(EnvironmentTest, PointOutOfBoundsTest) { {XYZCoord(10, 10, 0), XYZCoord(20, 10, 0), XYZCoord(20, 20, 0), XYZCoord(10, 20, 0)}}; std::vector obstacles = {obs1}; - Environment test{small_square, {XYZCoord(0, 0, 0)}, obstacles}; + Environment test{small_square, {}, {XYZCoord(0, 0, 0)}, obstacles}; EXPECT_EQ(true, test.isPointInBounds(XYZCoord{0.5, 0.5, 0})); EXPECT_EQ(true, test.isPointInBounds(XYZCoord{0.5, 0.5, 99999999})); @@ -90,7 +88,7 @@ TEST(EnvironmentTest, PointOutOfBoundsTest) { EXPECT_EQ(false, test.isPointInBounds(XYZCoord{0.5, -1, 0})); // down Polygon no_point_polygon; - Environment no_point = {no_point_polygon, {XYZCoord(0, 0, 0)}, obstacles}; + Environment no_point = {no_point_polygon, {}, {XYZCoord(0, 0, 0)}, obstacles}; EXPECT_EQ(false, no_point.isPointInBounds(XYZCoord{1, 1, 1})); EXPECT_EQ(false, no_point.isPointInBounds(XYZCoord{1, 0, 1})); @@ -99,7 +97,7 @@ TEST(EnvironmentTest, PointOutOfBoundsTest) { Polygon point_polygon; point_polygon.emplace_back(XYZCoord{1, 1, 1}); - Environment point = {point_polygon, {XYZCoord(0, 0, 0)}, obstacles}; + Environment point = {point_polygon, {}, {XYZCoord(0, 0, 0)}, obstacles}; EXPECT_EQ(false, point.isPointInBounds(XYZCoord{1, 1, 1})); EXPECT_EQ(false, point.isPointInBounds(XYZCoord{1, 0, 1})); @@ -112,7 +110,7 @@ TEST(EnvironmentTest, PointOutOfBoundsTest) { quadrateral_polygon.emplace_back(XYZCoord{4, 4, 0}); quadrateral_polygon.emplace_back(XYZCoord{1, 2, 0}); - Environment quadrilateral = {quadrateral_polygon, {XYZCoord(0, 0, 0)}, obstacles}; + Environment quadrilateral = {quadrateral_polygon, {}, {XYZCoord(0, 0, 0)}, obstacles}; EXPECT_EQ(true, quadrilateral.isPointInBounds(XYZCoord{1.5, 1.00, 0})); EXPECT_EQ(true, quadrilateral.isPointInBounds(XYZCoord{0.5, 0.90, 0})); @@ -144,7 +142,7 @@ TEST(EnvironmentTest, PathOutOfBoundsTest) { std::vector obstacles = {obs1}; - Environment test{small_square, {XYZCoord(0, 0, 0)}, obstacles}; + Environment test{small_square, {}, {XYZCoord(0, 0, 0)}, obstacles}; // TODO --> REALLY SHIT TEST std::vector path_in_bounds{XYZCoord{0.5, 0.5, 0}, XYZCoord{0.5, 0.5, 99999999}, @@ -161,8 +159,8 @@ TEST(EnvironmentTest, PathOutOfBoundsTest) { } /* -* test points in bound and in obstacles -*/ + * test points in bound and in obstacles + */ TEST(EnvironmentTest, InsideObstacleTest) { Polygon square; square.emplace_back(XYZCoord{0, 0, 0}); @@ -175,8 +173,8 @@ TEST(EnvironmentTest, InsideObstacleTest) { std::vector obstacles = {obs1}; - Environment test{square, {XYZCoord(0, 0, 0)}, obstacles}; - + Environment test{square, {}, {XYZCoord(0, 0, 0)}, obstacles}; + EXPECT_FALSE(test.isPointInBounds(XYZCoord{15, 15, 0})); EXPECT_FALSE(test.isPointInBounds(XYZCoord{10, 10, 0})); EXPECT_FALSE(test.isPointInBounds(XYZCoord{15, 10, 0})); @@ -186,14 +184,14 @@ TEST(EnvironmentTest, InsideObstacleTest) { } /* -* tests Environment::intersect() -*/ + * tests Environment::intersect() + */ TEST(EnvironmentTest, IntersectTest) { - Environment test({}, {}, {}); + Environment test({}, {}, {}, {}); // test intersect std::vector path1 = {XYZCoord{0, 0, 0}, XYZCoord{100, 100, 0}}; - std::vector path1_5 = {XYZCoord{0, 0, 0}, XYZCoord{-1, -1, 0}}; + std::vector path1_5 = {XYZCoord{0, 0, 0}, XYZCoord{-1, -1, 0}}; std::vector path2 = {XYZCoord{-1, -1, 0}, XYZCoord{5, 5, 0}}; std::vector path3 = {XYZCoord{0, 0, 0}, XYZCoord{15, 15, 0}}; std::vector path4 = {XYZCoord{2, 1, 0}, XYZCoord{1, 2, 0}}; @@ -206,6 +204,45 @@ TEST(EnvironmentTest, IntersectTest) { EXPECT_TRUE(test.intersect(path1[0], path1[1], path4[0], path4[1])); EXPECT_FALSE(test.intersect(path1_5[0], path1_5[1], path4[0], path4[1])); +} - +/* + * tests Environment::rayIntersectsEdge() + */ +TEST(EnvironmentTest, RayIntersectsEdge) { + Polygon airdrop_zone = { + {XYZCoord(0, 0, 0), XYZCoord(100, 0, 0), XYZCoord(100, 100, 0), XYZCoord(50, 100, 0)}}; + + Environment test{{}, airdrop_zone, {}, {}}; + + std::pair edge1 = {XYZCoord(75, 9999, 0), XYZCoord(75, -9999, 0)}; + XYZCoord intersect1(0, 0, 0); + XYZCoord expect1(75, 0, 0); + XYZCoord expect2(75, 100, 0); + + EXPECT_TRUE(test.rayIntersectsEdge(airdrop_zone[0], airdrop_zone[1], edge1.first, edge1.second, + intersect1)); + EXPECT_EQ(intersect1, expect1); + EXPECT_FALSE(test.rayIntersectsEdge(airdrop_zone[1], airdrop_zone[2], edge1.first, edge1.second, + intersect1)); + EXPECT_TRUE(test.rayIntersectsEdge(airdrop_zone[2], airdrop_zone[3], edge1.first, edge1.second, + intersect1)); + EXPECT_EQ(intersect1, expect2); + EXPECT_FALSE(test.rayIntersectsEdge(airdrop_zone[3], airdrop_zone[0], edge1.first, edge1.second, + intersect1)); + + std::pair edge2 = {XYZCoord(25, 9999, 0), XYZCoord(25, -9999, 0)}; + XYZCoord intersect2(0, 0, 0); + XYZCoord expect3(25, 0, 0); + XYZCoord expect4(25, 50, 0); + EXPECT_TRUE(test.rayIntersectsEdge(airdrop_zone[0], airdrop_zone[1], edge2.first, edge2.second, + intersect2)); + EXPECT_EQ(intersect2, expect3); + EXPECT_FALSE(test.rayIntersectsEdge(airdrop_zone[1], airdrop_zone[2], edge2.first, edge2.second, + intersect2)); + EXPECT_FALSE(test.rayIntersectsEdge(airdrop_zone[2], airdrop_zone[3], edge2.first, edge2.second, + intersect2)); + EXPECT_TRUE(test.rayIntersectsEdge(airdrop_zone[3], airdrop_zone[0], edge2.first, edge2.second, + intersect2)); + EXPECT_EQ(intersect2, expect4); } \ No newline at end of file diff --git a/tests/unit/pathing/tree_test.cpp b/tests/unit/pathing/tree_test.cpp index 9f4bf511..f8bd4683 100644 --- a/tests/unit/pathing/tree_test.cpp +++ b/tests/unit/pathing/tree_test.cpp @@ -25,7 +25,7 @@ TEST(SimpleTreeTest, addNodeTest) { {XYZCoord(10, 10, 0), XYZCoord(20, 10, 0), XYZCoord(20, 20, 0), XYZCoord(10, 20, 0)}}; std::vector obstacles = {obs1}; - Environment env = Environment(valid_region, {XYZCoord(0, 0, 0)}, obstacles); + Environment env = Environment(valid_region, {}, {XYZCoord(0, 0, 0)}, obstacles); RRTPoint point1 = RRTPoint(XYZCoord(25, 25, 0), 0); RRTPoint point2 = RRTPoint(XYZCoord(50, 75, 0), 0); RRTOption option = dubins.allOptions(point1, point2, true)[0]; @@ -53,7 +53,7 @@ TEST(SimpleTreeTest, rewireEdgeTest) { {XYZCoord(10, 10, 0), XYZCoord(20, 10, 0), XYZCoord(20, 20, 0), XYZCoord(10, 20, 0)}}; std::vector obstacles = {obs1}; - Environment env = Environment(valid_region, {XYZCoord(0, 0, 0)}, obstacles); + Environment env = Environment(valid_region, {}, {XYZCoord(0, 0, 0)}, obstacles); RRTPoint point1 = RRTPoint(XYZCoord(25, 25, 0), 0); RRTPoint point2 = RRTPoint(XYZCoord(50, 75, 0), HALF_PI); RRTPoint point3 = RRTPoint(XYZCoord(50, 80, 1.5), HALF_PI); diff --git a/tests/unit/pathing_test.cpp b/tests/unit/pathing_test.cpp index 20d9d2be..450fd5be 100644 --- a/tests/unit/pathing_test.cpp +++ b/tests/unit/pathing_test.cpp @@ -46,7 +46,7 @@ TEST(StaticPathingTest, RRTTest) { // actually new test // validate the path - Environment env(state->config.getFlightBoundary(), {}, {}); + Environment env(state->config.getFlightBoundary(), {}, {}, {}); std::vector path = state->getInitPath();