diff --git a/configs/dev.json b/configs/dev.json index e55ddd1f..c7ac9d88 100644 --- a/configs/dev.json +++ b/configs/dev.json @@ -28,7 +28,8 @@ "optimize": true, "point_fetch_method": "nearest", "allowed_to_skip_waypoints": false, - "generate_deviations": false + "generate_deviations": false, + "environment_offset": 10 }, "coverage": { "altitude_m": 30.0, diff --git a/include/pathing/environment.hpp b/include/pathing/environment.hpp index 892b6194..92da555b 100644 --- a/include/pathing/environment.hpp +++ b/include/pathing/environment.hpp @@ -23,7 +23,7 @@ class Environment { public: Environment(const Polygon& valid_region, const Polygon& airdrop_zone, const Polygon& mapping_region, const std::vector& goals, - const std::vector& obstacles); + const std::vector& obstacles, double environment_offset = 0); /** * Check if a point is in the valid region @@ -258,6 +258,15 @@ class Environment { std::pair estimateAreaCoveredAndPathLength( const std::vector& goals) const; + /** + * Find the centroid of a given valid region. + * This rests on the assumption that the region + * is in an non-intersecting order. + * + * @return the centroid coordinate + */ + Polygon scaleFixedDistance(double distance, const Polygon& shape) const; + /** * Returns a new polygon that is scaled by a given factor * @@ -273,6 +282,7 @@ class Environment { const Polygon mapping_region; // boundary of the mapping region (subset of valid_region) const std::vector goals; // goal point const std::vector obstacles; // obstacles in the map + const double environment_offset; int goals_found; // whether or not the goal has been found, once it becomes ture, it will never // be false again diff --git a/include/utilities/obc_config.hpp b/include/utilities/obc_config.hpp index 186555a1..05906def 100644 --- a/include/utilities/obc_config.hpp +++ b/include/utilities/obc_config.hpp @@ -68,6 +68,7 @@ struct RRTConfig { bool allowed_to_skip_waypoints; // if true, will skip waypoints if it can not connect after 1 // RRT iteration bool generate_deviations; + double environment_offset; }; namespace AirdropCoverageMethod { diff --git a/src/pathing/environment.cpp b/src/pathing/environment.cpp index 60f19393..e6ec70c9 100644 --- a/src/pathing/environment.cpp +++ b/src/pathing/environment.cpp @@ -12,14 +12,15 @@ Environment::Environment(const Polygon& valid_region, const Polygon& airdrop_zone, const Polygon& mapping_region, const std::vector& goals, - const std::vector& obstacles) + const std::vector& obstacles, double environment_offset) : valid_region(valid_region), airdrop_zone(airdrop_zone), mapping_region(mapping_region), goals(goals), goals_found(0), bounds(findBounds(valid_region)), - obstacles(obstacles) {} + obstacles(obstacles), + environment_offset(environment_offset) {} bool Environment::isPointInBounds(const XYZCoord& point) const { if (!isPointInPolygon(valid_region, point)) { @@ -492,3 +493,35 @@ std::pair, std::pair> Environment::fin return {{min_x, max_x}, {min_y, max_y}}; } + +Polygon Environment::scaleFixedDistance(double distance, const Polygon& shape) const { + Polygon reduced_polygon; + + // square bounds of the polygon + auto bounds = findBounds(shape); + auto [x_min, x_max] = bounds.first; + auto [y_min, y_max] = bounds.second; + + // finds the center of the polygon + double x_center = (x_max + x_min) / 2; + double y_center = (y_max + y_min) / 2; + + for (const XYZCoord& point : shape) { + double distance_x = (point.x - x_center); + double distance_y = (point.y - y_center); + if (distance_x > 0) { + distance_x -= distance; + } else { + distance_x += distance; + } + if (distance_y > 0) { + distance_y -= distance; + } else { + distance_y += distance; + } + double new_x_coord = x_center + distance_x; + double new_y_coord = y_center + distance_y; + reduced_polygon.push_back(XYZCoord(new_x_coord, new_y_coord, 0)); + } + return reduced_polygon; +} diff --git a/src/pathing/static.cpp b/src/pathing/static.cpp index edaf2b1b..76f9e378 100644 --- a/src/pathing/static.cpp +++ b/src/pathing/static.cpp @@ -25,8 +25,9 @@ RRT::RRT(RRTPoint start, std::vector goals, double search_radius, Poly : iterations_per_waypoint(config.pathing.rrt.iterations_per_waypoint), search_radius(search_radius), rewire_radius(config.pathing.rrt.rewire_radius), - tree(start, Environment(bounds, {}, {}, goals, obstacles), - Dubins(config.pathing.dubins.turning_radius, config.pathing.dubins.point_separation)), + tree(start, Environment(bounds, {}, {}, goals, obstacles, + config.pathing.rrt.environment_offset), + Dubins(config.pathing.dubins.turning_radius, config.pathing.dubins.point_separation)), config(config.pathing.rrt) { if (angles.size() != 0) { this->angles = angles; diff --git a/tests/unit/pathing/centroid_test.cpp b/tests/unit/pathing/centroid_test.cpp new file mode 100644 index 00000000..b8e26d82 --- /dev/null +++ b/tests/unit/pathing/centroid_test.cpp @@ -0,0 +1,29 @@ + +#include +#include "pathing/environment.hpp" +#include + +TEST(ScaleContour, Square) { + + std::vector contour { + {0.0, 0.0, 0.0}, + {10.0, 0.0, 0.0}, + {10.0, 10.0, 0.0}, + {0.0, 10.0, 0.0} + }; + + std::vector contour_scaled { + {2.0, 2.0, 0.0}, + {8.0, 2.0, 0.0}, + {8.0, 8.0, 0.0}, + {2.0, 8.0, 0.0} + }; + + Environment env = Environment({}, {}, {}, {}, {}); + Polygon pred_contour = env.scaleFixedDistance(2, contour); + + for (int i = 0; i < pred_contour.size(); ++i) { + EXPECT_EQ(pred_contour[i].x, contour_scaled[i].x); + EXPECT_EQ(pred_contour[i].y, contour_scaled[i].y); + } +}