Feat/scale env bounds

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,

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<XYZCoord>& goals,
-                const std::vector<Polygon>& obstacles);
+                const std::vector<Polygon>& obstacles, double environment_offset = 0);
 
     /**
      * Check if a point is in the valid region
@@ -258,6 +258,15 @@ class Environment {
     std::pair<double, double> estimateAreaCoveredAndPathLength(
         const std::vector<XYZCoord>& 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<XYZCoord> goals;  // goal point
     const std::vector<Polygon> 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

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 {

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<XYZCoord>& goals,
-                         const std::vector<Polygon>& obstacles)
+                         const std::vector<Polygon>& 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<double, double>, std::pair<double, double>> 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;
+}

src/pathing/static.cpp

@@ -25,8 +25,9 @@ RRT::RRT(RRTPoint start, std::vector<XYZCoord> 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;

tests/unit/pathing/centroid_test.cpp

@@ -0,0 +1,29 @@
+
+#include <gtest/gtest.h>
+#include "pathing/environment.hpp"
+#include <iostream>
+
+TEST(ScaleContour, Square) {
+
+    std::vector<XYZCoord> 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<XYZCoord> 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);
+    }
+}