Skip to content

Commit

Permalink
Environment Scaling, 'optimal' pathing, linting
Browse files Browse the repository at this point in the history
  • Loading branch information
@AskewParity
AskewParity committed Apr 17, 2024
1 parent 6872a21 commit e3281f0
Show file tree
Hide file tree
Showing 7 changed files with 66 additions and 60 deletions.
3 changes: 3 additions & 0 deletions include/pathing/static.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -180,6 +180,9 @@ class AirdropSearch {
/**
* Generates a path of parallel lines to cover a given area
*
* TODO - optimize dubins to not have to go to each line, rather search every other line then
* loop back
*
* @return ==> list of 2-vectors describing the path through the aridrop_zone
*/
std::vector<XYZCoord> run() const;
Expand Down
6 changes: 3 additions & 3 deletions include/utilities/datatypes.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -104,9 +104,9 @@ struct RRTConfig {
};

struct AirdropSearchConfig {
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
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
};

#endif // INCLUDE_UTILITIES_DATATYPES_HPP_
2 changes: 1 addition & 1 deletion protos
Submodule protos updated 1 files
+0 −8 obc.proto
53 changes: 29 additions & 24 deletions src/pathing/environment.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -238,8 +238,8 @@ std::vector<XYZCoord> Environment::getAirdropEndpoints(int scan_radius, bool ver

for (double coordinate = start; coordinate <= end; coordinate += iteration) {
// finds where this x-coordinate intersects with the airdrop zone (always convex)
XYZCoord top(0,0,0);
XYZCoord bottom(0,0,0);
XYZCoord top(0, 0, 0);
XYZCoord bottom(0, 0, 0);
if (vertical) {
top = XYZCoord(coordinate, y_max, 0);
bottom = XYZCoord(coordinate, y_min, 0);
Expand All @@ -248,33 +248,35 @@ std::vector<XYZCoord> Environment::getAirdropEndpoints(int scan_radius, bool ver
bottom = XYZCoord(x_max, -coordinate, 0);
}

std::vector<XYZCoord> intersections = findIntersections(airdrop_zone, top, bottom, vertical);
std::vector<XYZCoord> intersections =
findIntersections(airdrop_zone, top, bottom, vertical);

if (vertical) {
if (intersections[0].y < intersections[1].y) {
endpoints.push_back(intersections[1]);
endpoints.push_back(intersections[0]);
endpoints.push_back(intersections[1]);
endpoints.push_back(intersections[0]);

} else {
endpoints.push_back(intersections[0]);
endpoints.push_back(intersections[1]);
endpoints.push_back(intersections[0]);
endpoints.push_back(intersections[1]);
}
} else {
if (intersections[0].x < intersections[1].x) {
endpoints.push_back(intersections[0]);
endpoints.push_back(intersections[1]);
endpoints.push_back(intersections[0]);
endpoints.push_back(intersections[1]);

} else {
endpoints.push_back(intersections[1]);
endpoints.push_back(intersections[0]);
endpoints.push_back(intersections[1]);
endpoints.push_back(intersections[0]);
}
}
}

return endpoints;
}

std::vector<RRTPoint> Environment::getAirdropWaypoints(int scan_radius, bool one_way, bool vertical) const {
std::vector<RRTPoint> Environment::getAirdropWaypoints(int scan_radius, bool one_way,
bool vertical) const {
std::vector<RRTPoint> waypoints;
std::vector<XYZCoord> endpoints = getAirdropEndpoints(scan_radius, vertical);
double angle = vertical ? 3.0 / 2.0 * M_PI : 0;
Expand Down Expand Up @@ -302,11 +304,9 @@ std::vector<RRTPoint> Environment::getAirdropWaypoints(int scan_radius, bool one
return waypoints;
}



bool Environment::verticalRayIntersectsEdge(const XYZCoord& p1, const XYZCoord& p2,
const XYZCoord& rayStart, const XYZCoord& rayEnd,
XYZCoord& intersection) const {
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);
Expand All @@ -318,8 +318,8 @@ bool Environment::verticalRayIntersectsEdge(const XYZCoord& p1, const XYZCoord&
}

bool Environment::horizontalRayIntersectsEdge(const XYZCoord& p1, const XYZCoord& p2,
const XYZCoord& rayStart, const XYZCoord& rayEnd,
XYZCoord& intersection) const {
const XYZCoord& rayStart, const XYZCoord& rayEnd,
XYZCoord& intersection) const {
// if the x coordinate lines between the edge
if ((p2.y <= rayStart.y && p1.y >= rayStart.y) || (p1.y <= rayStart.y && p2.y >= rayStart.y)) {
double inverse_slope = (p2.x - p1.x) / (p2.y - p1.y);
Expand All @@ -330,8 +330,6 @@ bool Environment::horizontalRayIntersectsEdge(const XYZCoord& p1, const XYZCoord
return false;
}



std::vector<XYZCoord> Environment::findIntersections(const Polygon& polygon,
const XYZCoord& rayStart,
const XYZCoord& rayEnd, bool vertical) const {
Expand All @@ -345,9 +343,9 @@ std::vector<XYZCoord> Environment::findIntersections(const Polygon& polygon,

// temporary variable to be changed if an intersection is found
XYZCoord intersection(0, 0, 0);
bool found_intersection = vertical ?
verticalRayIntersectsEdge(p1, p2, rayStart, rayEnd, intersection)
: horizontalRayIntersectsEdge(p1, p2, rayStart, rayEnd, intersection);
bool found_intersection =
vertical ? verticalRayIntersectsEdge(p1, p2, rayStart, rayEnd, intersection)
: horizontalRayIntersectsEdge(p1, p2, rayStart, rayEnd, intersection);
if (found_intersection) {
intersections.push_back(intersection);
}
Expand All @@ -359,15 +357,20 @@ std::vector<XYZCoord> Environment::findIntersections(const Polygon& polygon,
Polygon Environment::scale(double scale, const Polygon& source_polygon) const {
Polygon scaled_polygon;

// square bounds of the polygon
auto bounds = findBounds(source_polygon);
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 : source_polygon) {
double scaled_x = (point.x - x_center) * scale + x_center;
// shifts the polygon to the center so scaling doesn't shift the centter (readability)
// scales the polygon
// retranslates the center to the original center
double scaled_x = (point.x - x_center) * scale + x_center;
double scaled_y = (point.y - y_center) * scale + y_center;

scaled_polygon.push_back(XYZCoord(scaled_x, scaled_y, 0));
Expand All @@ -382,11 +385,13 @@ std::pair<std::pair<double, double>, std::pair<double, double>> Environment::fin
return std::make_pair(std::make_pair(0, 0), std::make_pair(0, 0));
}

// initial values
double min_x = region[0].x;
double max_x = region[0].x;
double min_y = region[0].y;
double max_y = region[0].y;

// finds the min and max x and y values
for (const XYZCoord& point : region) {
min_x = std::min(min_x, point.x);
max_x = std::max(max_x, point.x);
Expand Down
32 changes: 15 additions & 17 deletions src/pathing/static.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -292,7 +292,7 @@ std::vector<XYZCoord> AirdropSearch::run() const {
// generates the path connecting the q
std::vector<XYZCoord> path;
RRTPoint current = start;
for (const RRTPoint & waypoint: waypoints) {
for (const RRTPoint &waypoint : waypoints) {
std::vector<XYZCoord> dubins_path = dubins.dubinsPath(current, waypoint);
path.insert(path.end(), dubins_path.begin() + 1, dubins_path.end());
current = waypoint;
Expand All @@ -315,17 +315,21 @@ std::vector<XYZCoord> AirdropSearch::run() const {
{false, true}, {false, false}, {true, true}, {true, false}};

std::vector<std::vector<RRTOption>> dubins_paths;
std::vector<int> lengths = {0, 0, 0, 0};

// generates the endpoints for the lines (including headings)
for (auto &config : configs) {
for (int i = 0; i < configs.size(); i++) {
const auto &config = configs[i];

std::vector<RRTPoint> waypoints =
airspace.getAirdropWaypoints(scan_radius, config.first, config.second);

// generates the path connecting the q
// generates the path connecting the waypoints to each other
std::vector<RRTOption> current_dubins_path;
RRTPoint current = start;
for (const RRTPoint& waypoint : waypoints) {
for (const RRTPoint &waypoint : waypoints) {
RRTOption dubins_path = dubins.bestOption(current, waypoint);
lengths[i] += dubins_path.length;
current_dubins_path.push_back(dubins_path);
current = waypoint;
}
Expand All @@ -335,15 +339,10 @@ std::vector<XYZCoord> AirdropSearch::run() const {

// finds the shortest path
int shortest_path_index = 0;
double shortest_length = std::numeric_limits<double>::max();
for (int i = 0; i < dubins_paths.size(); i++) {
double length = 0;
for (auto &option : dubins_paths[i]) {
length += option.length;
}

if (length < shortest_length) {
shortest_length = length;
double shortest_length = lengths[0];
for (int i = 1; i < lengths.size(); i++) {
if (lengths[i] < shortest_length) {
shortest_length = lengths[i];
shortest_path_index = i;
}
}
Expand All @@ -357,10 +356,9 @@ std::vector<XYZCoord> AirdropSearch::run() const {
scan_radius, configs[shortest_path_index].first, configs[shortest_path_index].second);

// initial path to the region is a special case, so we deal with it individually
std::vector<XYZCoord> init_path = dubins
.generatePoints(current, waypoints[0],
dubins_paths[shortest_path_index][0].dubins_path,
dubins_paths[shortest_path_index][0].has_straight);
std::vector<XYZCoord> init_path = dubins.generatePoints(
current, waypoints[0], dubins_paths[shortest_path_index][0].dubins_path,
dubins_paths[shortest_path_index][0].has_straight);
path.insert(path.end(), init_path.begin(), init_path.end());

// go through all the waypoints
Expand Down
10 changes: 5 additions & 5 deletions tests/integration/airdrop_pathing_2.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -225,11 +225,11 @@ const static char* mission_json_2024 = R"(

/*
* FILE OUTPUT LOCATIONS
* |- build
* |- pathing_output
* |- test_airdrop_pathing.jpg
* |- test_airdrop_pathing.gif (if enabled)
* |- airdop_search_coords.txt
* |-- 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
*/
Expand Down
20 changes: 10 additions & 10 deletions tests/unit/environment_test.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -207,9 +207,9 @@ TEST(EnvironmentTest, IntersectTest) {
}

/*
* tests Environment::rayIntersectsEdge()
* tests Environment::verticalRayIntersectsEdge()
*/
TEST(EnvironmentTest, RayIntersectsEdge) {
TEST(EnvironmentTest, VerticalRayIntersectsEdge) {
Polygon airdrop_zone = {
{XYZCoord(0, 0, 0), XYZCoord(100, 0, 0), XYZCoord(100, 100, 0), XYZCoord(50, 100, 0)}};

Expand All @@ -220,29 +220,29 @@ TEST(EnvironmentTest, RayIntersectsEdge) {
XYZCoord expect1(75, 0, 0);
XYZCoord expect2(75, 100, 0);

EXPECT_TRUE(test.rayIntersectsEdge(airdrop_zone[0], airdrop_zone[1], edge1.first, edge1.second,
EXPECT_TRUE(test.verticalRayIntersectsEdge(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,
EXPECT_FALSE(test.verticalRayIntersectsEdge(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,
EXPECT_TRUE(test.verticalRayIntersectsEdge(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,
EXPECT_FALSE(test.verticalRayIntersectsEdge(airdrop_zone[3], airdrop_zone[0], edge1.first, edge1.second,
intersect1));

std::pair<XYZCoord, XYZCoord> 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,
EXPECT_TRUE(test.verticalRayIntersectsEdge(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,
EXPECT_FALSE(test.verticalRayIntersectsEdge(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,
EXPECT_FALSE(test.verticalRayIntersectsEdge(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,
EXPECT_TRUE(test.verticalRayIntersectsEdge(airdrop_zone[3], airdrop_zone[0], edge2.first, edge2.second,
intersect2));
EXPECT_EQ(intersect2, expect4);
}

0 comments on commit e3281f0

Please sign in to comment.