From f4730593fd1dbc4ab2a43269a02c8a637e8877f4 Mon Sep 17 00:00:00 2001 From: David Soto Date: Fri, 22 May 2026 01:42:06 +0000 Subject: [PATCH] Finally got Counterclockwise test to work and adding all the values --- include/utilities/common.hpp | 8 ++++++ src/network/gcs_routes.cpp | 16 +++++++++-- src/utilities/common.cpp | 35 ++++++++++++++++++++++-- tests/integration/path_plotting.cpp | 2 +- tests/unit/pathing/tree_test.cpp | 41 ++++++++++++++++++++++++++++- 5 files changed, 96 insertions(+), 6 deletions(-) diff --git a/include/utilities/common.hpp b/include/utilities/common.hpp index 803da18e..d29e4f51 100644 --- a/include/utilities/common.hpp +++ b/include/utilities/common.hpp @@ -2,8 +2,16 @@ #define INCLUDE_UTILITIES_COMMON_HPP_ #include +#include +#include + +// #include +#include "protos/obc.pb.h" + std::chrono::seconds getUnixTime_s(); std::chrono::milliseconds getUnixTime_ms(); +int checkCounterClockwise(std::vector coords); + #endif // INCLUDE_UTILITIES_COMMON_HPP_ diff --git a/src/network/gcs_routes.cpp b/src/network/gcs_routes.cpp index 33eb3f0c..1daf8823 100644 --- a/src/network/gcs_routes.cpp +++ b/src/network/gcs_routes.cpp @@ -1,5 +1,6 @@ #include #include +#include #include #include @@ -21,6 +22,7 @@ #include "ticks/path_validate.hpp" #include "ticks/tick.hpp" #include "ticks/wait_for_takeoff.hpp" +#include "utilities/common.hpp" #include "utilities/http.hpp" #include "utilities/logging.hpp" #include "utilities/serialize.hpp" @@ -118,9 +120,20 @@ DEF_GCS_HANDLE(Post, mission) { Mission mission; google::protobuf::util::JsonStringToMessage(request.body, &mission); + state->setCartesianConverter(CartesianConverter(mission.waypoints())); + + std::vector coord_Vector( + mission.flightboundary().begin(), + mission.flightboundary().end() + ); + + if(checkCounterClockwise(coord_Vector) == 0){ + LOG_RESPONSE(ERROR, "Not CounterClockwise", BAD_REQUEST); + } + // Update the cartesian converter to be centered around the new flight boundary state->setCartesianConverter(CartesianConverter(mission.flightboundary())); - + auto err = state->mission_params.setMission(mission, state->getCartesianConverter().value()); if (err.has_value()) { LOG_RESPONSE(WARNING, err.value().c_str(), BAD_REQUEST); @@ -128,7 +141,6 @@ DEF_GCS_HANDLE(Post, mission) { LOG_RESPONSE(INFO, "Mission uploaded", OK); } } - DEF_GCS_HANDLE(Post, targets, locations) { LOG_REQUEST("POST", "/targets/locations"); diff --git a/src/utilities/common.cpp b/src/utilities/common.cpp index 6dc8c15f..b2b73b3a 100644 --- a/src/utilities/common.cpp +++ b/src/utilities/common.cpp @@ -1,10 +1,16 @@ -#include "utilities/common.hpp" - +#include #include #include #include #include "utilities/datatypes.hpp" +#include "pathing/mission_path.hpp" +#include "ticks/path_gen.hpp" +#include "pathing/mission_path.hpp" +#include "ticks/path_validate.hpp" +#include "network/gcs_routes.hpp" +#include "protos/obc.pb.h" + std::chrono::seconds getUnixTime_s() { const auto now = std::chrono::system_clock::now(); @@ -15,3 +21,28 @@ std::chrono::milliseconds getUnixTime_ms() { const auto now = std::chrono::system_clock::now(); return std::chrono::duration_cast(now.time_since_epoch()); } + +int checkCounterClockwise(std::vector coords) { + double tot_sign = 0; + double xnplus1 = 0; + double ynplus1 = 0; + for(int i = 0; i < coords.size(); i++){ + double xn = coords[i].latitude(); + double yn = coords[i].longitude(); + if(i == coords.size() - 1){ + xnplus1 = coords[0].latitude(); + ynplus1 = coords[0].longitude(); + } else { + xnplus1 = coords[i+1].latitude(); + ynplus1 = coords[i+1].longitude(); + } + tot_sign += ((xn *ynplus1) - (yn * xnplus1)); + } + std::cout << tot_sign; + if(tot_sign < 0){ + return 0; // 0 is counter clockwise + }else { + return 1; // 1 is not counterclockwise + } +} +//checks coordinates if they're in counter clockwise \ No newline at end of file diff --git a/tests/integration/path_plotting.cpp b/tests/integration/path_plotting.cpp index bef8ad11..d31f9417 100644 --- a/tests/integration/path_plotting.cpp +++ b/tests/integration/path_plotting.cpp @@ -24,4 +24,4 @@ int main() { plotter.output("test_static", PathOutputType::STATIC); plotter.output("test_animated", PathOutputType::ANIMATED); -} \ No newline at end of file +} diff --git a/tests/unit/pathing/tree_test.cpp b/tests/unit/pathing/tree_test.cpp index e491c576..a2cf5852 100644 --- a/tests/unit/pathing/tree_test.cpp +++ b/tests/unit/pathing/tree_test.cpp @@ -1,11 +1,12 @@ #include "pathing/tree.hpp" - +#include #include #include "pathing/dubins.hpp" #include "pathing/environment.hpp" #include "utilities/constants.hpp" #include "utilities/datatypes.hpp" +#include "utilities/common.hpp" /* * very bad tests, was too lazy to check if every parameter was correct, aka didn't bother to find @@ -13,6 +14,44 @@ */ #include +TEST(CheckCounterClockwiseTest, CounterClockwiseTest){ + // testing if flightbounds counter clockwise returns correctly + GPSCoord GPS4 = makeGPSCoord(0,0,0); + GPSCoord GPS3 = makeGPSCoord(10,0,0); + GPSCoord GPS2 = makeGPSCoord(10,10,0); + GPSCoord GPS1 = makeGPSCoord(0,10,0); + + std::vector coords_Test1 = {GPS1, GPS2, GPS3, GPS4}; + + int expected_val1 = checkCounterClockwise(coords_Test1); + + EXPECT_TRUE(expected_val1 == 0); + //test with actual coords + GPSCoord GEISEL = makeGPSCoord(32.88146, -117.23772,0); + std::cout << "If this appears coordinate1 was created propely" << std::endl; + GPSCoord PINES = makeGPSCoord(32.87886,-117.24249,0); + std::cout << "If this appears coordinate2 was created propely" << std::endl; + GPSCoord FAH = makeGPSCoord(32.88354,-117.23496,0); + std::cout << "If this appears coordinate3 was created propely" << std::endl; + GPSCoord SUNGOD = makeGPSCoord(32.87864,-117.23977,0); + std::cout << "If this appears coordinate4 was created propely" << std::endl; + + std::vector coords_Test2 = {GEISEL, PINES, SUNGOD, FAH}; + + std::cout << "If this appears coord list was created properly" << std::endl; + + int expected_val2 = checkCounterClockwise(coords_Test2); + std::cout << "If this appears Triple C ran" << std::endl; + + EXPECT_TRUE(expected_val2 == 0); + + std::vector coords_Test3 = {GEISEL, SUNGOD, PINES, FAH}; + + int expected_val3 = checkCounterClockwise(coords_Test3); + + EXPECT_TRUE(expected_val3 == 1); +} + TEST(SimpleTreeTest, addNodeTest) { Dubins dubins{5, 0.1}; Polygon valid_region;