diff --git a/CMakeLists.txt b/CMakeLists.txt index 532d9de3..d4a10224 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -184,7 +184,7 @@ if(CPPLINT) # also don't check for runtime/references since Google's public style guidelines are behind https://github.com/cpplint/cpplint/issues/148 --exclude=../include/udp_squared --exclude=../src/utilities/base64.cpp - --filter=-legal,-readability/todo,-build/c++11,-runtime/references + --filter=-legal,-readability/todo,-build/c++11,-build/c++17,-runtime/references --linelength=100 --recursive ../src diff --git a/include/network/serialize.hpp b/include/network/serialize.hpp index 91d1cb2d..cf82e9aa 100644 --- a/include/network/serialize.hpp +++ b/include/network/serialize.hpp @@ -8,19 +8,19 @@ */ namespace serialh { - template - void serialize(T* response, boost::asio::streambuf* buf) { - std::ostream os(buf); - boost::archive::binary_oarchive oa(os); - oa << *response; - } +template +void serialize(T* response, boost::asio::streambuf* buf) { + std::ostream os(buf); + boost::archive::binary_oarchive oa(os); + oa << *response; +} - template - void deserialize(T* response, boost::asio::streambuf* buf) { - std::istream is(buf); - boost::archive::binary_iarchive ia(is); - ia >> *response; - } +template +void deserialize(T* response, boost::asio::streambuf* buf) { + std::istream is(buf); + boost::archive::binary_iarchive ia(is); + ia >> *response; +} } // namespace serialh #endif // INCLUDE_NETWORK_SERIALIZE_HPP_ diff --git a/include/network/udp_client.hpp b/include/network/udp_client.hpp index 4bc52583..39010aba 100644 --- a/include/network/udp_client.hpp +++ b/include/network/udp_client.hpp @@ -8,7 +8,7 @@ #include #include #include -#include "rpi_connection.hpp" +#include "network/rpi_connection.hpp" namespace asio = boost::asio; diff --git a/include/pathing/dubins.hpp b/include/pathing/dubins.hpp index 53f7ecce..9023c542 100644 --- a/include/pathing/dubins.hpp +++ b/include/pathing/dubins.hpp @@ -62,7 +62,7 @@ double mod(double dividend, double divisor); * @param second ==> second option * @return ==> true if @param first has a smaller length */ -bool compareRRTOptionLength(const RRTOption &first, const RRTOption &second); +bool compareRRTOptionLength(const RRTOption& first, const RRTOption& second); /** * Finds a orthogonal 2-vector to the 2-vector inputted @@ -72,7 +72,7 @@ bool compareRRTOptionLength(const RRTOption &first, const RRTOption &second); * counter-clockwise) * @see https://mathworld.wolfram.com/PerpendicularVector.html */ -XYZCoord findOrthogonalVector2D(const XYZCoord &vector); +XYZCoord findOrthogonalVector2D(const XYZCoord& vector); /** * returns half of the displacement vector from v2 to v1 @@ -83,233 +83,227 @@ XYZCoord findOrthogonalVector2D(const XYZCoord &vector); * @param vector1 that terminates half way * (i.e. half the magnitude) */ -XYZCoord halfDisplacement(const XYZCoord &vector1, const XYZCoord &vector2); +XYZCoord halfDisplacement(const XYZCoord& vector1, const XYZCoord& vector2); -class Dubins { - public: - Dubins(double radius, double point_separation); +namespace Dubins { +inline double _radius; +inline double _point_separation; - /** - * Finds the center of a given turn orignating at a vector turning left or right - * Assumes the path may be done perfectly circularly - * - * @param point ==> current position of the plane (vector) with psi in radians - * @param side ==> whether the plane is planning to turn left (L) or right (R) - * @return ==> center of a turning circle - */ - XYZCoord findCenter(const RRTPoint &point, char side) const; +/** + * Finds the center of a given turn orignating at a vector turning left or right + * Assumes the path may be done perfectly circularly + * + * @param point ==> current position of the plane (vector) with psi in radians + * @param side ==> whether the plane is planning to turn left (L) or right (R) + * @return ==> center of a turning circle + */ +XYZCoord findCenter(const RRTPoint& point, char side); - /** - * Finds a point (vector) along a curved path given a distance - * already traveled - * - * @param starting_point ==> position vector of the plane (only psi is - * used to ascertain direction) - * @param beta ==> angle of the turn (positive is left/ccw) - * @param center ==> center of the circle - * @param path_length ==> the arc-length along the circle - * @returns ==> point along circle path - */ - XYZCoord circleArc(const RRTPoint &starting_point, double beta, const XYZCoord ¢er, - double path_length) const; - /** - * Generates points for the paths that contain a straight section - * [LSL, LSR, RSR, RSL] - * - * @param start ==> current "vector" of the plane - * @param end ==> final/desired "vector" of the path - * @param path ==> Some given dubins's path parameters - * @return ==> a list of vectors along the entire path - */ - std::vector generatePointsStraight(const RRTPoint &start, const RRTPoint &end, - const DubinsPath &path) const; +/** + * Finds a point (vector) along a curved path given a distance + * already traveled + * + * @param starting_point ==> position vector of the plane (only psi is + * used to ascertain direction) + * @param beta ==> angle of the turn (positive is left/ccw) + * @param center ==> center of the circle + * @param path_length ==> the arc-length along the circle + * @returns ==> point along circle path + */ +XYZCoord circleArc(const RRTPoint& starting_point, double beta, const XYZCoord& center, + double path_length); +/** + * Generates points for the paths that contain a straight section + * [LSL, LSR, RSR, RSL] + * + * @param start ==> current "vector" of the plane + * @param end ==> final/desired "vector" of the path + * @param path ==> Some given dubins's path parameters + * @return ==> a list of vectors along the entire path + */ +std::vector generatePointsStraight(const RRTPoint& start, const RRTPoint& end, + const DubinsPath& path); - /** - * Generates points for the paths that only contain curved sections - * [RLR, LRL] - * - * @param start ==> current "vector" of the plane - * @param end ==> final/desired "vector" of the path - * @param path ==> Some given dubins's path parameters - * @return ==> a list of vectors along the entire path - */ - std::vector generatePointsCurve(const RRTPoint &start, const RRTPoint &end, - const DubinsPath &path) const; +/** + * Generates points for the paths that only contain curved sections + * [RLR, LRL] + * + * @param start ==> current "vector" of the plane + * @param end ==> final/desired "vector" of the path + * @param path ==> Some given dubins's path parameters + * @return ==> a list of vectors along the entire path + */ +std::vector generatePointsCurve(const RRTPoint& start, const RRTPoint& end, + const DubinsPath& path); - /** - * Abstraction for generating points (curved/straight) - * - * @param start ==> vector at start position - * @param end ==> vector at end position - * @param path ==> DubinsPath encoding turning and straight information - * @param has_straigt ==> whether the given DubinsPath has a straight section or not - * @return ==> a list of points that represent the shortest - * dubin's path from start to end - */ - std::vector generatePoints(const RRTPoint &start, const RRTPoint &end, - const DubinsPath &path, bool has_straight) const; +/** + * Abstraction for generating points (curved/straight) + * + * @param start ==> vector at start position + * @param end ==> vector at end position + * @param path ==> DubinsPath encoding turning and straight information + * @param has_straigt ==> whether the given DubinsPath has a straight section or not + * @return ==> a list of points that represent the shortest + * dubin's path from start to end + */ +std::vector generatePoints(const RRTPoint& start, const RRTPoint& end, + const DubinsPath& path, bool has_straight); - /** - * First, the straight distance (it turns out) is equal to the - * distance between the two centers - * Next it calculates the turning angle (positive [CCW] turn from start to - * end vector) - * - * @param start ==> vector at start position - * @param end ==> vector at end position - * @param center_0 ==> the center of the first turn - * @param center_2 ==> the center of the last turn - * @return RRTOption detailing the parameters of the path - * - total distance - * - DubinsPath - * - turning angle 1 - * - turning angle 2 - * - straight_distance - * - if the path has a straight section - */ - RRTOption lsl(const RRTPoint &start, const RRTPoint &end, const XYZCoord ¢er_0, - const XYZCoord ¢er_2) const; +/** + * First, the straight distance (it turns out) is equal to the + * distance between the two centers + * Next it calculates the turning angle (positive [CCW] turn from start to + * end vector) + * + * @param start ==> vector at start position + * @param end ==> vector at end position + * @param center_0 ==> the center of the first turn + * @param center_2 ==> the center of the last turn + * @return RRTOption detailing the parameters of the path + * - total distance + * - DubinsPath + * - turning angle 1 + * - turning angle 2 + * - straight_distance + * - if the path has a straight section + */ +RRTOption lsl(const RRTPoint& start, const RRTPoint& end, const XYZCoord& center_0, + const XYZCoord& center_2); - /** - * First, the straight distance (it turns out) is equal to the - * distance between the two centers - * Next it calculates the turning angle (negative [CW] turn from start to end vector) - * - * @param start ==> vector at start position - * @param end ==> vector at end position - * @param center_0 ==> the center of the first turn - * @param center_2 ==> the center of the last turn - * @return RRTOption detailing the parameters of the path - * - total distance - * - DubinsPath - * - turning angle 1 - * - turning angle 2 - * - straight_distance - * - if the path has a straight section - */ - RRTOption rsr(const RRTPoint &start, const RRTPoint &end, const XYZCoord ¢er_0, - const XYZCoord ¢er_2) const; +/** + * First, the straight distance (it turns out) is equal to the + * distance between the two centers + * Next it calculates the turning angle (negative [CW] turn from start to end vector) + * + * @param start ==> vector at start position + * @param end ==> vector at end position + * @param center_0 ==> the center of the first turn + * @param center_2 ==> the center of the last turn + * @return RRTOption detailing the parameters of the path + * - total distance + * - DubinsPath + * - turning angle 1 + * - turning angle 2 + * - straight_distance + * - if the path has a straight section + */ +RRTOption rsr(const RRTPoint& start, const RRTPoint& end, const XYZCoord& center_0, + const XYZCoord& center_2); - /** - * Because of the change in turn direction, it is a little more complex to - * compute than in the RSR or LSL cases. - * - * The tangent line between both turns turns out to bisect the intercenter. - * With that knowledge, it is possible to compute both the length and the angle of the - * tangent line ==> then allowing the calculation for the proper turning angles. - * - * @param start ==> vector at start position - * @param end ==> vector at end position - * @param center_0 ==> the center of the first turn - * @param center_2 ==> the center of the last turn - * @return RRTOption detailing the parameters of the path - * - total distance - * - DubinsPath - * - turning angle 1 - * - turning angle 2 - * - straight_distance - * - if the path has a straight section - */ - RRTOption lsr(const RRTPoint &start, const RRTPoint &end, const XYZCoord ¢er_0, - const XYZCoord ¢er_2) const; +/** + * Because of the change in turn direction, it is a little more complex to + * compute than in the RSR or LSL cases. + * + * The tangent line between both turns turns out to bisect the intercenter. + * With that knowledge, it is possible to compute both the length and the angle of the + * tangent line ==> then allowing the calculation for the proper turning angles. + * + * @param start ==> vector at start position + * @param end ==> vector at end position + * @param center_0 ==> the center of the first turn + * @param center_2 ==> the center of the last turn + * @return RRTOption detailing the parameters of the path + * - total distance + * - DubinsPath + * - turning angle 1 + * - turning angle 2 + * - straight_distance + * - if the path has a straight section + */ +RRTOption lsr(const RRTPoint& start, const RRTPoint& end, const XYZCoord& center_0, + const XYZCoord& center_2); - /** - * Because of the change in turn direction, it is a little more complex to - * compute than in the RSR or LSL cases. - * - * The tangent line between both turns turns out to bisect the intercenter. - * With that knowledge, it is possible to compute both the length and the angle of the - * tangent line ==> then allowing the calculation for the proper turning angles. - * - * @param start ==> vector at start position - * @param end ==> vector at end position - * @param center_0 ==> the center of the first turn - * @param center_2 ==> the center of the last turn - * @return RRTOption detailing the parameters of the path - * - total distance - * - DubinsPath - * - turning angle 1 - * - turning angle 2 - * - straight_distance - * - if the path has a straight section - */ - RRTOption rsl(const RRTPoint &start, const RRTPoint &end, const XYZCoord ¢er_0, - const XYZCoord ¢er_2) const; +/** + * Because of the change in turn direction, it is a little more complex to + * compute than in the RSR or LSL cases. + * + * The tangent line between both turns turns out to bisect the intercenter. + * With that knowledge, it is possible to compute both the length and the angle of the + * tangent line ==> then allowing the calculation for the proper turning angles. + * + * @param start ==> vector at start position + * @param end ==> vector at end position + * @param center_0 ==> the center of the first turn + * @param center_2 ==> the center of the last turn + * @return RRTOption detailing the parameters of the path + * - total distance + * - DubinsPath + * - turning angle 1 + * - turning angle 2 + * - straight_distance + * - if the path has a straight section + */ +RRTOption rsl(const RRTPoint& start, const RRTPoint& end, const XYZCoord& center_0, + const XYZCoord& center_2); - /** - * Using the isoceles triangle made by the centers of the three circles, - * computes the required angles. - * - * @param start ==> vector at start position - * @param end ==> vector at end position - * @param center_0 ==> the center of the first turn - * @param center_2 ==> the center of the last turn - * @return RRTOption detailing the parameters of the path - * - total distance - * - DubinsPath - * - turning angle 1 - * - turning angle 2 - * - straight_distance - * - if the path has a straight section - */ - RRTOption lrl(const RRTPoint &start, const RRTPoint &end, const XYZCoord ¢er_0, - const XYZCoord ¢er_2) const; +/** + * Using the isoceles triangle made by the centers of the three circles, + * computes the required angles. + * + * @param start ==> vector at start position + * @param end ==> vector at end position + * @param center_0 ==> the center of the first turn + * @param center_2 ==> the center of the last turn + * @return RRTOption detailing the parameters of the path + * - total distance + * - DubinsPath + * - turning angle 1 + * - turning angle 2 + * - straight_distance + * - if the path has a straight section + */ +RRTOption lrl(const RRTPoint& start, const RRTPoint& end, const XYZCoord& center_0, + const XYZCoord& center_2); - /** - * Using the isoceles triangle made by the centers of the three circles, - * computes the required angles. - * - * @param start ==> vector at start position - * @param end ==> vector at end position - * @param center_0 ==> the center of the first turn - * @param center_2 ==> the center of the last turn - * @return RRTOption detailing the parameters of the path - * - total distance - * - DubinsPath - * - turning angle 1 - * - turning angle 2 - * - straight_distance - * - if the path has a straight section - */ - RRTOption rlr(const RRTPoint &start, const RRTPoint &end, const XYZCoord ¢er_0, - const XYZCoord ¢er_2) const; +/** + * Using the isoceles triangle made by the centers of the three circles, + * computes the required angles. + * + * @param start ==> vector at start position + * @param end ==> vector at end position + * @param center_0 ==> the center of the first turn + * @param center_2 ==> the center of the last turn + * @return RRTOption detailing the parameters of the path + * - total distance + * - DubinsPath + * - turning angle 1 + * - turning angle 2 + * - straight_distance + * - if the path has a straight section + */ +RRTOption rlr(const RRTPoint& start, const RRTPoint& end, const XYZCoord& center_0, + const XYZCoord& center_2); - /** - * Compute all the possible Dubin's path and returns a list - * containing RRTOption(s) with path data. - * - * @param start ==> vector at start position - * @param end ==> vector at end position - * @param sort ==> whether the method sorts the resulting vector - * DEFALT-->FALSE (searching is faster) - * @return ==> list containing all the RRTOptions from the path - * generation - */ - std::vector allOptions(const RRTPoint &start, const RRTPoint &end, - bool sort = false) const; +/** + * Compute all the possible Dubin's path and returns a list + * containing RRTOption(s) with path data. + * + * @param start ==> vector at start position + * @param end ==> vector at end position + * @return ==> list containing all the RRTOptions from the path + * generation + */ +std::vector allOptions(const RRTPoint& start, const RRTPoint& end); - /** - * Compute all the possible Dubin's path(s) and - * Returns sequence of points representing the shortest option. - * - * @param start ==> vector at start position - * @param end ==> vector at end position - * @return ==> the points for the most optimal path from @param start to @param end - */ - std::vector dubinsPath(const RRTPoint &start, const RRTPoint &end) const; +/** + * Compute all the possible Dubin's path(s) and + * Returns sequence of points representing the shortest option. + * + * @param start ==> vector at start position + * @param end ==> vector at end position + * @return ==> the points for the most optimal path from @param start to @param end + */ +std::vector dubinsPath(const RRTPoint& start, const RRTPoint& end); - /** - * Returns the optimal RRTOption from the list of options - * - * @param start ==> vector at start position - * @param end ==> vector at end position - * @return ==> the optimal RRTOption from the list of options - */ - RRTOption bestOption(const RRTPoint &start, const RRTPoint &end) const; +/** + * Returns the optimal RRTOption from the list of options + * + * @param start ==> vector at start position + * @param end ==> vector at end position + * @return ==> the optimal RRTOption from the list of options + */ +RRTOption bestOption(const RRTPoint& start, const RRTPoint& end); - private: - const double _radius; - const double _point_separation; -}; +}; // namespace Dubins #endif // INCLUDE_PATHING_DUBINS_HPP_ diff --git a/include/pathing/static.hpp b/include/pathing/static.hpp index b8a615b9..5284b24b 100644 --- a/include/pathing/static.hpp +++ b/include/pathing/static.hpp @@ -241,7 +241,6 @@ class ForwardCoveragePathing { // 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 const AirdropCoverageConfig config; }; @@ -288,7 +287,6 @@ class AirdropApproachPathing { const XYZCoord goal; const RRTPoint start; const Environment airspace; - const Dubins dubins; const OBCConfig config; XYZCoord wind; diff --git a/include/pathing/tree.hpp b/include/pathing/tree.hpp index 73ff603f..a3a1f6c0 100644 --- a/include/pathing/tree.hpp +++ b/include/pathing/tree.hpp @@ -104,7 +104,7 @@ class RRTNode { class RRTTree { public: - RRTTree(RRTPoint root_point, Environment airspace, Dubins dubins); + RRTTree(RRTPoint root_point, Environment airspace); ~RRTTree(); /** @@ -284,7 +284,6 @@ class RRTTree { std::shared_ptr root; std::shared_ptr current_head; Environment airspace; - Dubins dubins; int tree_size; /** diff --git a/include/utilities/http.hpp b/include/utilities/http.hpp index 6196540b..86c1d4dd 100644 --- a/include/utilities/http.hpp +++ b/include/utilities/http.hpp @@ -33,8 +33,8 @@ constexpr const char* HTTP_STATUS_TO_STRING(HTTPStatus status) { } namespace mime { - const char json[] = "application/json"; - const char plaintext[] = "text/plain"; +const char json[] = "application/json"; +const char plaintext[] = "text/plain"; } #endif // INCLUDE_UTILITIES_HTTP_HPP_ diff --git a/src/camera/interface.cpp b/src/camera/interface.cpp index 8b3939bd..92b660d4 100644 --- a/src/camera/interface.cpp +++ b/src/camera/interface.cpp @@ -1,8 +1,11 @@ #include "camera/interface.hpp" #include +#include #include #include +#include +#include #include #include "nlohmann/json.hpp" diff --git a/src/camera/mock.cpp b/src/camera/mock.cpp index 730c3a61..32b3998e 100644 --- a/src/camera/mock.cpp +++ b/src/camera/mock.cpp @@ -3,10 +3,13 @@ #include #include -#include -#include #include #include +#include +#include +#include +#include +#include #include #include "nlohmann/json.hpp" diff --git a/src/camera/rpi.cpp b/src/camera/rpi.cpp index 082ce2d3..27aa147a 100644 --- a/src/camera/rpi.cpp +++ b/src/camera/rpi.cpp @@ -1,9 +1,11 @@ #include -#include +#include +#include #include #include +#include +#include #include -#include #include #include diff --git a/src/core/mission_parameters.cpp b/src/core/mission_parameters.cpp index dac734bf..64c0a22f 100644 --- a/src/core/mission_parameters.cpp +++ b/src/core/mission_parameters.cpp @@ -5,8 +5,10 @@ #include #include #include +#include #include #include +#include #include "pathing/cartesian.hpp" #include "protos/obc.pb.h" diff --git a/src/core/mission_state.cpp b/src/core/mission_state.cpp index ca5f38fa..0e581d43 100644 --- a/src/core/mission_state.cpp +++ b/src/core/mission_state.cpp @@ -2,6 +2,8 @@ #include #include +#include +#include #include #include "camera/interface.hpp" diff --git a/src/core/obc.cpp b/src/core/obc.cpp index c699b3f6..3afaa218 100644 --- a/src/core/obc.cpp +++ b/src/core/obc.cpp @@ -1,8 +1,9 @@ -#include -#include #include #include #include +#include +#include +#include #include "camera/rpi.hpp" #include "camera/mock.hpp" diff --git a/src/cv/aggregator.cpp b/src/cv/aggregator.cpp index bfce6146..dc4a0e09 100644 --- a/src/cv/aggregator.cpp +++ b/src/cv/aggregator.cpp @@ -1,6 +1,11 @@ #include "cv/aggregator.hpp" #include +#include +#include +#include +#include +#include #include "utilities/constants.hpp" #include "utilities/lockptr.hpp" diff --git a/src/cv/clustering.cpp b/src/cv/clustering.cpp index fcfcfe17..58dc0214 100644 --- a/src/cv/clustering.cpp +++ b/src/cv/clustering.cpp @@ -1,5 +1,6 @@ -#include #include +#include +#include #include "protos/obc.pb.h" #include "cv/localization.hpp" #include "cv/clustering.hpp" diff --git a/src/cv/hdr.cpp b/src/cv/hdr.cpp index 4e2cfe39..e39e8a96 100644 --- a/src/cv/hdr.cpp +++ b/src/cv/hdr.cpp @@ -1,5 +1,8 @@ #include "cv/hdr.hpp" +#include +#include + std::vector generate_test_images() { std::vector img_list; std::vector colors = { diff --git a/src/cv/localization.cpp b/src/cv/localization.cpp index 8574e310..458bf32f 100644 --- a/src/cv/localization.cpp +++ b/src/cv/localization.cpp @@ -1,314 +1,316 @@ -#include "cv/localization.hpp" -#include - -#define PI 3.14159265 - -// struct ECEFCoordinates; -// struct CameraVector; -// struct ENUCoordinates; - -ECEFLocalization::ECEFCoordinates ECEFLocalization::GPStoECEF(GPSCoord gps) { - double a = 6378137; // Earth semi-major axis in meters - double b = 6356752; // Earth semi-minor axis in meters - double e2 = 1 - (b*b)/(a*a); - ECEFCoordinates ecef; - ecef.x = (gps.altitude() + a/ - (sqrt(1-e2*sin(gps.latitude())*sin(gps.latitude()))))* - cos(gps.latitude())*cos(gps.longitude()); - ecef.y = (gps.altitude() + a/ - (sqrt(1-e2*sin(gps.latitude())*sin(gps.latitude()))))* - cos(gps.latitude())*sin(gps.longitude()); - ecef.z = (gps.altitude() + (1-e2)*a/ - (sqrt(1-e2*sin(gps.latitude())*sin(gps.latitude()))))*sin(gps.latitude()); - return ecef; -} - -// Converts a GPS location and ENU offset to ECEF coordinates -ECEFLocalization::ECEFCoordinates ECEFLocalization::ENUtoECEF( - ENUCoordinates offset, GPSCoord originGPS) { - ECEFCoordinates origin = GPStoECEF(originGPS); - ECEFCoordinates target; - target.x = origin.x - sin(originGPS.longitude())*offset.e - - sin(originGPS.latitude())*cos(originGPS.longitude())*offset.n + - cos(originGPS.latitude())*cos(originGPS.longitude())*offset.u; - target.y = origin.y + cos(originGPS.longitude())*offset.e - - sin(originGPS.latitude())*sin(originGPS.longitude())*offset.n + - cos(originGPS.latitude())*sin(originGPS.longitude())*offset.u; - target.z = origin.z + cos(originGPS.longitude())*offset.n + sin(originGPS.latitude())*offset.u; - return target; -} - -// Converts ECEF cooordinates to GPS coordinates using Heikkinen's procedure -// Dont use. Shit doesnt work :sob: -GPSCoord ECEFLocalization::ECEFtoGPS(ECEFCoordinates ecef) { - GPSCoord gps; - double a = EARTH_RADIUS_METERS; - double b = 6356752; - double e2 = 1 - ((b*b)/(a*a)); - double ep2 = ((a*a)/(b*b)) - 1; - double p = sqrt((ecef.x*ecef.x) + (ecef.y*ecef.y)); - double F = 54*(b*b)*(ecef.z*ecef.z); - double G = (p*p) + ((1 - e2)*(ecef.z*ecef.z)) - (e2*(a*a - b*b)); - double c = e2*e2*F*p*p/(G*G*G); - double s = cbrt(1 + c + sqrt((c*c) + (2*c))); - double k = s + 1 + (1/s); - double P = F/(3*k*k*G*G); - double Q = sqrt(1 + (2*e2*e2*P)); - double r0 = (-P*e2*p/(1 + Q)) + sqrt((0.5*a*a*(1 + (1/Q))) - - (P*(1 - e2)*ecef.z*ecef.z/(Q*(1 + Q))) - (0.5*P*p*p)); - double U = sqrt((p - (e2*r0))*(p - (e2*r0)) + (ecef.z*ecef.z)); - double V = sqrt((p - (e2*r0))*(p - (e2*r0)) + ((1 - e2)*ecef.z*ecef.z)); - double z0 = b*b*ecef.z/(a*V); - gps.set_latitude(atan((ecef.z + ep2*z0)/p)); - gps.set_longitude(atan2(ecef.y, ecef.x)); - gps.set_altitude(U*(1 - ((b*b)/(a*V)))); - return gps; -} - -// Calculate angle offset based on target pixel coordinates using pinhole camera model -ECEFLocalization::CameraVector ECEFLocalization::PixelsToAngle( - CameraIntrinsics camera, CameraVector state, double targetX, double targetY) { - CameraVector target; - target.roll = atan(camera.pixelSize*(targetX - (camera.resolutionX/2))/camera.focalLength); - target.pitch = atan(camera.pixelSize*(targetY - (camera.resolutionY/2))/camera.focalLength); - target.heading = state.heading; - return target; -} - -// Calculate the ENU offset of the intersection of a vector from -// the plane to the ground (assume flat) -ECEFLocalization::ENUCoordinates ECEFLocalization::AngleToENU( - CameraVector target, GPSCoord aircraft, double terrainHeight) { - double x = aircraft.altitude()*tan(target.roll); - double y = aircraft.altitude()*tan(target.pitch); - ENUCoordinates offset; - offset.e = x*cos(target.heading) + y*sin(target.heading); - offset.n = -x*sin(target.heading) + y*cos(target.heading); - offset.u = terrainHeight - aircraft.altitude(); - return offset; -} - -GPSCoord ECEFLocalization::localize(const ImageTelemetry& telemetry, const Bbox& targetBbox) { - double terrainHeight = 0; - - double targetX = (targetBbox.x1 + targetBbox.x2)/2; - double targetY = (targetBbox.y1 + targetBbox.y2)/2; - - - CameraVector gimbalState; - gimbalState.roll = telemetry.roll_deg*PI/180; - gimbalState.pitch = telemetry.pitch_deg*PI/180; - gimbalState.heading = telemetry.yaw_deg*PI/180; - - GPSCoord aircraft; - aircraft.set_latitude(telemetry.latitude_deg*PI/180); - aircraft.set_longitude(telemetry.longitude_deg*PI/180); - aircraft.set_altitude(telemetry.altitude_agl_m*1000); - - CameraVector targetVector = PixelsToAngle(camera, gimbalState, targetX, targetY); - ENUCoordinates offset = AngleToENU(targetVector, aircraft, terrainHeight); - ECEFCoordinates targetLocationECEF = ENUtoECEF(offset, aircraft); - GPSCoord targetLocationGPS = ECEFtoGPS(targetLocationECEF); - - double lat = targetLocationGPS.latitude()*180/PI; - double lon = targetLocationGPS.longitude()*180/PI; - double alt = targetLocationGPS.altitude()/1000; - - GPSCoord targetCoord; - targetCoord.set_latitude(lat); - targetCoord.set_longitude(lon); - targetCoord.set_altitude(alt); - return targetCoord; -} - - -GPSCoord GSDLocalization::localize(const ImageTelemetry& telemetry, const Bbox& targetBbox) { - GPSCoord gps; - - // Ground Sample Distance (mm/pixel), 1.0~2.5cm per px is ideal aka 10mm~25mm ppx - // double GSD = (SENSOR_WIDTH * (telemetry.altitude_agl_m * 1000)) - // / (FOCAL_LENGTH_MM * IMG_WIDTH_PX); - - std::cout << "Pre-localization: " << telemetry.latitude_deg << " " << telemetry.longitude_deg; - - double GSD = (ANGLE_OF_VIEW_RATIO * (telemetry.altitude_agl_m * 1000) - /IMG_WIDTH_PX); - - // Midpoints of the image - double img_mid_x = IMG_WIDTH_PX / 2; - double img_mid_y = IMG_HEIGHT_PX / 2; - - // midpoints of bounding box around the target - double target_x = (targetBbox.x1 + targetBbox.x2)/2; - double target_y = (targetBbox.y1 + targetBbox.y2)/2; - - // calculations of bearing - // L = (distance(middle, bbox))*GSD - double length = (sqrt(pow((target_x - img_mid_x), 2) + pow((target_y - img_mid_y), 2) * GSD)); - - // Translate Image Cordinates to Camera Cordinate (Origin to Center of Image instead of Top Left) NOLINT - double target_camera_cord_x = target_x - (IMG_WIDTH_PX / 2); - double target_camera_cord_y = (IMG_HEIGHT_PX / 2) - target_y; - - // Convert to polar coordinates - double target_camera_cord_r = sqrt((target_camera_cord_y * target_camera_cord_y) - + (target_camera_cord_x * target_camera_cord_x)); - double target_camera_cord_theta; - - // Check if xy coord is in quadrant 2 or 3, if so need to add pi - // (atan returns a value in the range -π/2 to π/2 radians) - // also check for if x coord == 0, if so just set theta to pi or -pi to - // (avoid divison by 0 in the atan function) - if (target_camera_cord_x < 0 && target_camera_cord_y < 0) { - target_camera_cord_theta = atan(target_camera_cord_y/target_camera_cord_x) + M_PI; - - } else if (target_camera_cord_x < 0 && target_camera_cord_y > 0) { - target_camera_cord_theta = atan(target_camera_cord_y/target_camera_cord_x) + M_PI; - - } else if (target_camera_cord_x == 0) { - if (target_camera_cord_y > 1) { - target_camera_cord_theta == M_PI; - } else { - target_camera_cord_theta == -M_PI; - } - - } else { - target_camera_cord_theta = atan(target_camera_cord_y/target_camera_cord_x); - } - - // Transfrom the coordinate to real-world orientation by subtracting heading angle - double hdg_radians = telemetry.heading_deg * M_PI / 180; - target_camera_cord_theta = target_camera_cord_theta - hdg_radians; - - // Convert back to regular coordinates - target_camera_cord_x = target_camera_cord_r*cos(target_camera_cord_theta); - target_camera_cord_y = target_camera_cord_r*sin(target_camera_cord_theta); - - // Finds the offset of the bbox - double calc_cam_offset_x_m = target_camera_cord_x * GSD * 0.001; // mm to M - double calc_cam_offset_y_m = target_camera_cord_y * GSD * 0.001; // mm to M - - // Calculates the cordinates using the offset - GPSCoord calc_coord = CalcOffset((calc_cam_offset_x_m), (calc_cam_offset_y_m), - (telemetry.latitude_deg), (telemetry.longitude_deg)); - - return calc_coord; -} - -/* -Takes the in two cordinaates and outputs their distance in meters. - -Parameters: -- lat1/lon1 (First Cordinate) -- lat2/lon2 (Second Cordinate) - -@returns distance in meters - -Reference: http://www.movable-type.co.uk/scripts/latlong.html -*/ - -double GSDLocalization::distanceInMetersBetweenCords(const double lat1, const double lon1, const double lat2, const double lon2) { // NOLINT - double e1 = lat1 * M_PI / 180; - double e2 = lat2 * M_PI / 180; - - double d1 = (lat2 - lat1) * M_PI / 180; - double d2 = (lon2 - lon1) * M_PI / 180; - - double a = sin(d1/2) * sin(d1/2) + cos(e1) * cos(e2) * sin(d2/2) * sin(d2/2); - - double c = 2 * atan2(sqrt(a), sqrt(1-a)); - - double d = EARTH_RADIUS_M * c; - - return d; -} - -/* -Takes the position of the camera in blender and the position of the generated target in meters - -Parameters: --image_offset_x/y - meters from center of plane (0,0) --cam_lat/lon - Set cordinates of plane - -@returns true (mostly) world cordinate of target -*/ - -GPSCoord GSDLocalization::CalcOffset(const double offset_x, const double offset_y, const double lat, const double lon) { // NOLINT - double dLat = offset_y / EARTH_RADIUS_M; - double dLon = offset_x / (EARTH_RADIUS_M * cos(M_PI * lat / 180)); - - double latO = lat + dLat * 180/M_PI; - double lonO = lon + dLon * 180/M_PI; - - GPSCoord output; - - output.set_latitude(latO); - output.set_longitude(lonO); - - return output; -} - -std::tuple -GSDLocalization::debug(const ImageTelemetry& telemetry, const Bbox& targetBbox) { - GPSCoord gps; - - // Ground Sample Distance (mm/pixel), 1.0~2.5cm per px is ideal aka 10mm~25mm ppx - // double GSD = (SENSOR_WIDTH * (telemetry.altitude_agl_m * 1000)) - // / (FOCAL_LENGTH_MM * IMG_WIDTH_PX); - - double GSD = (ANGLE_OF_VIEW_RATIO * (telemetry.altitude_agl_m * 1000) - /IMG_WIDTH_PX); - - // Midpoints of the image - double img_mid_x = IMG_WIDTH_PX / 2; - double img_mid_y = IMG_HEIGHT_PX / 2; - - // midpoints of bounding box around the target - double target_x = (targetBbox.x1 + targetBbox.x2)/2; - double target_y = (targetBbox.y1 + targetBbox.y2)/2; - - // calculations of bearing - // L = (distance(middle, bbox))*GSD - double length = (sqrt(pow((target_x - img_mid_x), 2) + pow((target_y - img_mid_y), 2) * GSD)); - - // Translate Image Cordinates to Camera Cordinate (Origin to Center of Image instead of Top Left) NOLINT - double target_camera_cord_x = target_x - (IMG_WIDTH_PX / 2); - double target_camera_cord_y = (IMG_HEIGHT_PX / 2) - target_y; - - // Convert to polar coordinates - double target_camera_cord_r = - sqrt((target_camera_cord_y * target_camera_cord_y) - + (target_camera_cord_x * target_camera_cord_x)); - double target_camera_cord_theta; - - // Check if xy coord is in quadrant 2 or 3, - // f so need to add pi (atan returns a value in the range -π/2 to π/2 radians) - // also check for if x coord == 0, - // if so just set theta to pi or -pi to avoid divison by 0 in the atan function - if (target_camera_cord_x < 0 && target_camera_cord_y < 0) { - target_camera_cord_theta = atan(target_camera_cord_y/target_camera_cord_x) + M_PI; - - } else if (target_camera_cord_x < 0 && target_camera_cord_y > 0) { - target_camera_cord_theta = atan(target_camera_cord_y/target_camera_cord_x) + M_PI; - - } else if (target_camera_cord_x == 0) { - if (target_camera_cord_y > 1) { - target_camera_cord_theta == M_PI; - } else { - target_camera_cord_theta == -M_PI; - } - } else { - target_camera_cord_theta = atan(target_camera_cord_y/target_camera_cord_x); - } - - // Transfrom the coordinate to real-world orientation by subtracting heading angle - double hdg_radians = (telemetry.heading_deg) * M_PI / 180; - target_camera_cord_theta = target_camera_cord_theta - hdg_radians; - // Convert back to regular coordinates - target_camera_cord_x = target_camera_cord_r*cos(target_camera_cord_theta); - target_camera_cord_y = target_camera_cord_r*sin(target_camera_cord_theta); - // Finds the offset of the bbox - double calc_cam_offset_x_m = target_camera_cord_x * GSD * 0.001; // mm to M - double calc_cam_offset_y_m = target_camera_cord_y * GSD * 0.001; // mm to M - return std::make_tuple(GSD, calc_cam_offset_x_m, calc_cam_offset_y_m); -} +#include "cv/localization.hpp" +#include +#include +#include + +#define PI 3.14159265 + +// struct ECEFCoordinates; +// struct CameraVector; +// struct ENUCoordinates; + +ECEFLocalization::ECEFCoordinates ECEFLocalization::GPStoECEF(GPSCoord gps) { + double a = 6378137; // Earth semi-major axis in meters + double b = 6356752; // Earth semi-minor axis in meters + double e2 = 1 - (b*b)/(a*a); + ECEFCoordinates ecef; + ecef.x = (gps.altitude() + a/ + (sqrt(1-e2*sin(gps.latitude())*sin(gps.latitude()))))* + cos(gps.latitude())*cos(gps.longitude()); + ecef.y = (gps.altitude() + a/ + (sqrt(1-e2*sin(gps.latitude())*sin(gps.latitude()))))* + cos(gps.latitude())*sin(gps.longitude()); + ecef.z = (gps.altitude() + (1-e2)*a/ + (sqrt(1-e2*sin(gps.latitude())*sin(gps.latitude()))))*sin(gps.latitude()); + return ecef; +} + +// Converts a GPS location and ENU offset to ECEF coordinates +ECEFLocalization::ECEFCoordinates ECEFLocalization::ENUtoECEF( + ENUCoordinates offset, GPSCoord originGPS) { + ECEFCoordinates origin = GPStoECEF(originGPS); + ECEFCoordinates target; + target.x = origin.x - sin(originGPS.longitude())*offset.e - + sin(originGPS.latitude())*cos(originGPS.longitude())*offset.n + + cos(originGPS.latitude())*cos(originGPS.longitude())*offset.u; + target.y = origin.y + cos(originGPS.longitude())*offset.e - + sin(originGPS.latitude())*sin(originGPS.longitude())*offset.n + + cos(originGPS.latitude())*sin(originGPS.longitude())*offset.u; + target.z = origin.z + cos(originGPS.longitude())*offset.n + sin(originGPS.latitude())*offset.u; + return target; +} + +// Converts ECEF cooordinates to GPS coordinates using Heikkinen's procedure +// Dont use. Shit doesnt work :sob: +GPSCoord ECEFLocalization::ECEFtoGPS(ECEFCoordinates ecef) { + GPSCoord gps; + double a = EARTH_RADIUS_METERS; + double b = 6356752; + double e2 = 1 - ((b*b)/(a*a)); + double ep2 = ((a*a)/(b*b)) - 1; + double p = sqrt((ecef.x*ecef.x) + (ecef.y*ecef.y)); + double F = 54*(b*b)*(ecef.z*ecef.z); + double G = (p*p) + ((1 - e2)*(ecef.z*ecef.z)) - (e2*(a*a - b*b)); + double c = e2*e2*F*p*p/(G*G*G); + double s = cbrt(1 + c + sqrt((c*c) + (2*c))); + double k = s + 1 + (1/s); + double P = F/(3*k*k*G*G); + double Q = sqrt(1 + (2*e2*e2*P)); + double r0 = (-P*e2*p/(1 + Q)) + sqrt((0.5*a*a*(1 + (1/Q))) - + (P*(1 - e2)*ecef.z*ecef.z/(Q*(1 + Q))) - (0.5*P*p*p)); + double U = sqrt((p - (e2*r0))*(p - (e2*r0)) + (ecef.z*ecef.z)); + double V = sqrt((p - (e2*r0))*(p - (e2*r0)) + ((1 - e2)*ecef.z*ecef.z)); + double z0 = b*b*ecef.z/(a*V); + gps.set_latitude(atan((ecef.z + ep2*z0)/p)); + gps.set_longitude(atan2(ecef.y, ecef.x)); + gps.set_altitude(U*(1 - ((b*b)/(a*V)))); + return gps; +} + +// Calculate angle offset based on target pixel coordinates using pinhole camera model +ECEFLocalization::CameraVector ECEFLocalization::PixelsToAngle( + CameraIntrinsics camera, CameraVector state, double targetX, double targetY) { + CameraVector target; + target.roll = atan(camera.pixelSize*(targetX - (camera.resolutionX/2))/camera.focalLength); + target.pitch = atan(camera.pixelSize*(targetY - (camera.resolutionY/2))/camera.focalLength); + target.heading = state.heading; + return target; +} + +// Calculate the ENU offset of the intersection of a vector from +// the plane to the ground (assume flat) +ECEFLocalization::ENUCoordinates ECEFLocalization::AngleToENU( + CameraVector target, GPSCoord aircraft, double terrainHeight) { + double x = aircraft.altitude()*tan(target.roll); + double y = aircraft.altitude()*tan(target.pitch); + ENUCoordinates offset; + offset.e = x*cos(target.heading) + y*sin(target.heading); + offset.n = -x*sin(target.heading) + y*cos(target.heading); + offset.u = terrainHeight - aircraft.altitude(); + return offset; +} + +GPSCoord ECEFLocalization::localize(const ImageTelemetry& telemetry, const Bbox& targetBbox) { + double terrainHeight = 0; + + double targetX = (targetBbox.x1 + targetBbox.x2)/2; + double targetY = (targetBbox.y1 + targetBbox.y2)/2; + + + CameraVector gimbalState; + gimbalState.roll = telemetry.roll_deg*PI/180; + gimbalState.pitch = telemetry.pitch_deg*PI/180; + gimbalState.heading = telemetry.yaw_deg*PI/180; + + GPSCoord aircraft; + aircraft.set_latitude(telemetry.latitude_deg*PI/180); + aircraft.set_longitude(telemetry.longitude_deg*PI/180); + aircraft.set_altitude(telemetry.altitude_agl_m*1000); + + CameraVector targetVector = PixelsToAngle(camera, gimbalState, targetX, targetY); + ENUCoordinates offset = AngleToENU(targetVector, aircraft, terrainHeight); + ECEFCoordinates targetLocationECEF = ENUtoECEF(offset, aircraft); + GPSCoord targetLocationGPS = ECEFtoGPS(targetLocationECEF); + + double lat = targetLocationGPS.latitude()*180/PI; + double lon = targetLocationGPS.longitude()*180/PI; + double alt = targetLocationGPS.altitude()/1000; + + GPSCoord targetCoord; + targetCoord.set_latitude(lat); + targetCoord.set_longitude(lon); + targetCoord.set_altitude(alt); + return targetCoord; +} + + +GPSCoord GSDLocalization::localize(const ImageTelemetry& telemetry, const Bbox& targetBbox) { + GPSCoord gps; + + // Ground Sample Distance (mm/pixel), 1.0~2.5cm per px is ideal aka 10mm~25mm ppx + // double GSD = (SENSOR_WIDTH * (telemetry.altitude_agl_m * 1000)) + // / (FOCAL_LENGTH_MM * IMG_WIDTH_PX); + + std::cout << "Pre-localization: " << telemetry.latitude_deg << " " << telemetry.longitude_deg; + + double GSD = (ANGLE_OF_VIEW_RATIO * (telemetry.altitude_agl_m * 1000) + /IMG_WIDTH_PX); + + // Midpoints of the image + double img_mid_x = IMG_WIDTH_PX / 2; + double img_mid_y = IMG_HEIGHT_PX / 2; + + // midpoints of bounding box around the target + double target_x = (targetBbox.x1 + targetBbox.x2)/2; + double target_y = (targetBbox.y1 + targetBbox.y2)/2; + + // calculations of bearing + // L = (distance(middle, bbox))*GSD + double length = (sqrt(pow((target_x - img_mid_x), 2) + pow((target_y - img_mid_y), 2) * GSD)); + + // Translate Image Cordinates to Camera Cordinate (Origin to Center of Image instead of Top Left) NOLINT + double target_camera_cord_x = target_x - (IMG_WIDTH_PX / 2); + double target_camera_cord_y = (IMG_HEIGHT_PX / 2) - target_y; + + // Convert to polar coordinates + double target_camera_cord_r = sqrt((target_camera_cord_y * target_camera_cord_y) + + (target_camera_cord_x * target_camera_cord_x)); + double target_camera_cord_theta; + + // Check if xy coord is in quadrant 2 or 3, if so need to add pi + // (atan returns a value in the range -π/2 to π/2 radians) + // also check for if x coord == 0, if so just set theta to pi or -pi to + // (avoid divison by 0 in the atan function) + if (target_camera_cord_x < 0 && target_camera_cord_y < 0) { + target_camera_cord_theta = atan(target_camera_cord_y/target_camera_cord_x) + M_PI; + + } else if (target_camera_cord_x < 0 && target_camera_cord_y > 0) { + target_camera_cord_theta = atan(target_camera_cord_y/target_camera_cord_x) + M_PI; + + } else if (target_camera_cord_x == 0) { + if (target_camera_cord_y > 1) { + target_camera_cord_theta == M_PI; + } else { + target_camera_cord_theta == -M_PI; + } + + } else { + target_camera_cord_theta = atan(target_camera_cord_y/target_camera_cord_x); + } + + // Transfrom the coordinate to real-world orientation by subtracting heading angle + double hdg_radians = telemetry.heading_deg * M_PI / 180; + target_camera_cord_theta = target_camera_cord_theta - hdg_radians; + + // Convert back to regular coordinates + target_camera_cord_x = target_camera_cord_r*cos(target_camera_cord_theta); + target_camera_cord_y = target_camera_cord_r*sin(target_camera_cord_theta); + + // Finds the offset of the bbox + double calc_cam_offset_x_m = target_camera_cord_x * GSD * 0.001; // mm to M + double calc_cam_offset_y_m = target_camera_cord_y * GSD * 0.001; // mm to M + + // Calculates the cordinates using the offset + GPSCoord calc_coord = CalcOffset((calc_cam_offset_x_m), (calc_cam_offset_y_m), + (telemetry.latitude_deg), (telemetry.longitude_deg)); + + return calc_coord; +} + +/* +Takes the in two cordinaates and outputs their distance in meters. + +Parameters: +- lat1/lon1 (First Cordinate) +- lat2/lon2 (Second Cordinate) + +@returns distance in meters + +Reference: http://www.movable-type.co.uk/scripts/latlong.html +*/ + +double GSDLocalization::distanceInMetersBetweenCords(const double lat1, const double lon1, const double lat2, const double lon2) { // NOLINT + double e1 = lat1 * M_PI / 180; + double e2 = lat2 * M_PI / 180; + + double d1 = (lat2 - lat1) * M_PI / 180; + double d2 = (lon2 - lon1) * M_PI / 180; + + double a = sin(d1/2) * sin(d1/2) + cos(e1) * cos(e2) * sin(d2/2) * sin(d2/2); + + double c = 2 * atan2(sqrt(a), sqrt(1-a)); + + double d = EARTH_RADIUS_M * c; + + return d; +} + +/* +Takes the position of the camera in blender and the position of the generated target in meters + +Parameters: +-image_offset_x/y - meters from center of plane (0,0) +-cam_lat/lon - Set cordinates of plane + +@returns true (mostly) world cordinate of target +*/ + +GPSCoord GSDLocalization::CalcOffset(const double offset_x, const double offset_y, const double lat, const double lon) { // NOLINT + double dLat = offset_y / EARTH_RADIUS_M; + double dLon = offset_x / (EARTH_RADIUS_M * cos(M_PI * lat / 180)); + + double latO = lat + dLat * 180/M_PI; + double lonO = lon + dLon * 180/M_PI; + + GPSCoord output; + + output.set_latitude(latO); + output.set_longitude(lonO); + + return output; +} + +std::tuple +GSDLocalization::debug(const ImageTelemetry& telemetry, const Bbox& targetBbox) { + GPSCoord gps; + + // Ground Sample Distance (mm/pixel), 1.0~2.5cm per px is ideal aka 10mm~25mm ppx + // double GSD = (SENSOR_WIDTH * (telemetry.altitude_agl_m * 1000)) + // / (FOCAL_LENGTH_MM * IMG_WIDTH_PX); + + double GSD = (ANGLE_OF_VIEW_RATIO * (telemetry.altitude_agl_m * 1000) + /IMG_WIDTH_PX); + + // Midpoints of the image + double img_mid_x = IMG_WIDTH_PX / 2; + double img_mid_y = IMG_HEIGHT_PX / 2; + + // midpoints of bounding box around the target + double target_x = (targetBbox.x1 + targetBbox.x2)/2; + double target_y = (targetBbox.y1 + targetBbox.y2)/2; + + // calculations of bearing + // L = (distance(middle, bbox))*GSD + double length = (sqrt(pow((target_x - img_mid_x), 2) + pow((target_y - img_mid_y), 2) * GSD)); + + // Translate Image Cordinates to Camera Cordinate (Origin to Center of Image instead of Top Left) NOLINT + double target_camera_cord_x = target_x - (IMG_WIDTH_PX / 2); + double target_camera_cord_y = (IMG_HEIGHT_PX / 2) - target_y; + + // Convert to polar coordinates + double target_camera_cord_r = + sqrt((target_camera_cord_y * target_camera_cord_y) + + (target_camera_cord_x * target_camera_cord_x)); + double target_camera_cord_theta; + + // Check if xy coord is in quadrant 2 or 3, + // f so need to add pi (atan returns a value in the range -π/2 to π/2 radians) + // also check for if x coord == 0, + // if so just set theta to pi or -pi to avoid divison by 0 in the atan function + if (target_camera_cord_x < 0 && target_camera_cord_y < 0) { + target_camera_cord_theta = atan(target_camera_cord_y/target_camera_cord_x) + M_PI; + + } else if (target_camera_cord_x < 0 && target_camera_cord_y > 0) { + target_camera_cord_theta = atan(target_camera_cord_y/target_camera_cord_x) + M_PI; + + } else if (target_camera_cord_x == 0) { + if (target_camera_cord_y > 1) { + target_camera_cord_theta == M_PI; + } else { + target_camera_cord_theta == -M_PI; + } + } else { + target_camera_cord_theta = atan(target_camera_cord_y/target_camera_cord_x); + } + + // Transfrom the coordinate to real-world orientation by subtracting heading angle + double hdg_radians = (telemetry.heading_deg) * M_PI / 180; + target_camera_cord_theta = target_camera_cord_theta - hdg_radians; + // Convert back to regular coordinates + target_camera_cord_x = target_camera_cord_r*cos(target_camera_cord_theta); + target_camera_cord_y = target_camera_cord_r*sin(target_camera_cord_theta); + // Finds the offset of the bbox + double calc_cam_offset_x_m = target_camera_cord_x * GSD * 0.001; // mm to M + double calc_cam_offset_y_m = target_camera_cord_y * GSD * 0.001; // mm to M + return std::make_tuple(GSD, calc_cam_offset_x_m, calc_cam_offset_y_m); +} diff --git a/src/cv/mapping.cpp b/src/cv/mapping.cpp index 784549bd..6e7de81b 100644 --- a/src/cv/mapping.cpp +++ b/src/cv/mapping.cpp @@ -9,6 +9,7 @@ #include #include #include +#include #include diff --git a/src/cv/pipeline.cpp b/src/cv/pipeline.cpp index c0239106..25cfa45e 100644 --- a/src/cv/pipeline.cpp +++ b/src/cv/pipeline.cpp @@ -1,6 +1,9 @@ #include "cv/pipeline.hpp" #include +#include +#include +#include #include "protos/obc.pb.h" #include "utilities/logging.hpp" diff --git a/src/cv/utilities.cpp b/src/cv/utilities.cpp index c3d83633..7786216f 100644 --- a/src/cv/utilities.cpp +++ b/src/cv/utilities.cpp @@ -1,4 +1,7 @@ #include "cv/utilities.hpp" + +#include + #include "utilities/logging.hpp" int Bbox::width() const { return x2 - x1; } diff --git a/src/cv/yolo.cpp b/src/cv/yolo.cpp index aa32422b..58fab0cf 100644 --- a/src/cv/yolo.cpp +++ b/src/cv/yolo.cpp @@ -2,7 +2,10 @@ #include #include +#include +#include #include +#include // For simplicity, we are not doing advanced error handling // Make sure to catch and handle exceptions in production code diff --git a/src/network/airdrop_client.cpp b/src/network/airdrop_client.cpp index c5374ca9..90d4b3de 100644 --- a/src/network/airdrop_client.cpp +++ b/src/network/airdrop_client.cpp @@ -1,6 +1,7 @@ #include "network/airdrop_client.hpp" #include +#include extern "C" { #include "network/airdrop_sockets.h" diff --git a/src/network/gcs_routes.cpp b/src/network/gcs_routes.cpp index 3d260d19..c4d1278a 100644 --- a/src/network/gcs_routes.cpp +++ b/src/network/gcs_routes.cpp @@ -1,10 +1,14 @@ #include #include +#include #include +#include +#include #include #include #include +#include #include #include diff --git a/src/network/mavlink.cpp b/src/network/mavlink.cpp index 3dba2099..476e9d31 100644 --- a/src/network/mavlink.cpp +++ b/src/network/mavlink.cpp @@ -16,6 +16,7 @@ #include #include #include +#include #include "core/mission_state.hpp" #include "pathing/mission_path.hpp" diff --git a/src/network/udp_client.cpp b/src/network/udp_client.cpp index 549b4a0f..be61ce87 100644 --- a/src/network/udp_client.cpp +++ b/src/network/udp_client.cpp @@ -1,6 +1,8 @@ #include "network/udp_client.hpp" #include #include +#include +#include UDPClient::UDPClient(asio::io_context* io_context_, std::string ip, int port) : socket_(*io_context_) { diff --git a/src/network/udp_server.cpp b/src/network/udp_server.cpp index ef0c2a9d..0650fbbe 100644 --- a/src/network/udp_server.cpp +++ b/src/network/udp_server.cpp @@ -1,4 +1,8 @@ +#include #include +#include +#include +#include #include "network/udp_server.hpp" // TODO: Didn't touch this for the most part since its for mocking. diff --git a/src/pathing/dubins.cpp b/src/pathing/dubins.cpp index 84286921..1807d3e4 100644 --- a/src/pathing/dubins.cpp +++ b/src/pathing/dubins.cpp @@ -1,8 +1,10 @@ #include "pathing/dubins.hpp" +#include #include #include #include +#include #include "utilities/datatypes.hpp" @@ -31,41 +33,30 @@ * if right turn ==> -beta) */ +enum turn { FIRST_TURN, MIDDLE_TURN, LAST_TURN }; + template int sign(T number) { return (T(0) < number) - (number < T(0)); } double mod(double dividend, double divisor) { - const double cpp_mod = std::fmod(dividend, divisor); - - if (cpp_mod < -0.001) { - return cpp_mod + divisor; - } - - return cpp_mod; - // return std::fmod(std::fmod(dividend, divisor) + divisor, divisor); + return std::fmod(std::fmod(dividend, divisor) + divisor, divisor); } -bool compareRRTOptionLength(const RRTOption &first, const RRTOption &second) { +bool compareRRTOptionLength(const RRTOption& first, const RRTOption& second) { return first.length < second.length; } -XYZCoord findOrthogonalVector2D(const XYZCoord &vector) { +XYZCoord findOrthogonalVector2D(const XYZCoord& vector) { return XYZCoord{-vector.y, vector.x, vector.z}; } -XYZCoord halfDisplacement(const XYZCoord &vector1, const XYZCoord &vector2) { +XYZCoord halfDisplacement(const XYZCoord& vector1, const XYZCoord& vector2) { return 0.5 * (vector1 - vector2); } -Dubins::Dubins(double radius, double point_separation) - : _radius(radius), _point_separation(point_separation) { - assert(radius > 0); - assert(point_separation > 0); -} - -XYZCoord Dubins::findCenter(const RRTPoint &point, char side) const { +XYZCoord Dubins::findCenter(const RRTPoint& point, char side) { assert(side == 'L' || side == 'R'); // creates a right angle between the RRTPoint vector towards the center @@ -74,11 +65,11 @@ XYZCoord Dubins::findCenter(const RRTPoint &point, char side) const { // creates the vector offset from the existing position return XYZCoord{point.coord.x + (std::cos(angle) * _radius), - point.coord.y + (std::sin(angle) * _radius), 0}; + point.coord.y + (std::sin(angle) * _radius), 0}; } -XYZCoord Dubins::circleArc(const RRTPoint &starting_point, double beta, const XYZCoord ¢er, - double path_length) const { +XYZCoord Dubins::circleArc(const RRTPoint& starting_point, double beta, const XYZCoord& center, + double path_length) { // Code is not nessisarily intuitive (I don't want to call sign twice) // starting_angle_(+- half_pi depending on the sign) + the_angular_distance_tranveled_(depending // on the sign) @@ -87,8 +78,8 @@ XYZCoord Dubins::circleArc(const RRTPoint &starting_point, double beta, const XY return center + (direction_vector * _radius); } -std::vector Dubins::generatePointsStraight(const RRTPoint &start, const RRTPoint &end, - const DubinsPath &path) const { +std::vector Dubins::generatePointsStraight(const RRTPoint& start, const RRTPoint& end, + const DubinsPath& path) { // the arclength of both curved sections + straight section double total_distance = _radius * (std::abs(path.beta_0) + std::abs(path.beta_2)) + path.straight_dist; @@ -123,27 +114,15 @@ std::vector Dubins::generatePointsStraight(const RRTPoint &start, cons double distance_straight = initial_terminal_point.distanceTo(final_terminal_point); // generates the points for the entire curve. + int n_points = std::max(1, static_cast(std::ceil(total_distance / _point_separation))); std::vector points_list; + points_list.reserve(n_points + 1); for (double current_distance = 0; current_distance < total_distance; current_distance += _point_separation) { if (current_distance < std::abs(path.beta_0) * _radius) { // First turn points_list.emplace_back(circleArc(start, path.beta_0, center_0, current_distance)); } else if (current_distance > total_distance - std::abs(path.beta_2) * _radius) { // Last turn - // OPTION 1: - // need to calculate new "start" point, which is the difference in end angle to turn - // angle RRTPoint final_point_RRT{XYZCoord{final_terminal_point.x, - // final_terminal_point.y, 0}, end.psi- path.beta_2}; last section is how much distance - // is covered in second turn points_list.emplace_back(circleArc(final_point_RRT, - // path.beta_2, center_2, current_distance - (total_distance - std::abs(path.beta_2) * - // _radius))); - - // OPTION 2: - // old code, more consise, and clever geometry, but less intuitive - // the distance is the max negative distance - // and it gets smaller, - // so that the angle starts out at - // final_terminal_point and moves towards the back of end points_list.emplace_back( circleArc(end, path.beta_2, center_2, current_distance - total_distance)); } else { // Straignt Section @@ -163,8 +142,8 @@ std::vector Dubins::generatePointsStraight(const RRTPoint &start, cons return points_list; } -std::vector Dubins::generatePointsCurve(const RRTPoint &start, const RRTPoint &end, - const DubinsPath &path) const { +std::vector Dubins::generatePointsCurve(const RRTPoint& start, const RRTPoint& end, + const DubinsPath& path) { // the arclength of all paths double total_distance = _radius * (std::abs(path.beta_2) + std::abs(path.beta_0) + std::abs(path.straight_dist)); @@ -190,7 +169,9 @@ std::vector Dubins::generatePointsCurve(const RRTPoint &start, const R // interior angle] double psi_0 = std::atan2(center_1.y - center_0.y, center_1.x - center_0.x) - M_PI; + int n_points = std::max(1, static_cast(std::ceil(total_distance / _point_separation))); std::vector points_list; + points_list.reserve(n_points + 1); for (double current_distance = 0; current_distance < total_distance; current_distance += _point_separation) { if (current_distance < std::abs(path.beta_0) * _radius) { // First Turn @@ -216,8 +197,8 @@ std::vector Dubins::generatePointsCurve(const RRTPoint &start, const R return points_list; } -std::vector Dubins::generatePoints(const RRTPoint &start, const RRTPoint &end, - const DubinsPath &path, bool has_straight) const { +std::vector Dubins::generatePoints(const RRTPoint& start, const RRTPoint& end, + const DubinsPath& path, bool has_straight) { if (has_straight) { return generatePointsStraight(start, end, path); } @@ -225,8 +206,8 @@ std::vector Dubins::generatePoints(const RRTPoint &start, const RRTPoi return generatePointsCurve(start, end, path); } -RRTOption Dubins::lsl(const RRTPoint &start, const RRTPoint &end, const XYZCoord ¢er_0, - const XYZCoord ¢er_2) const { +RRTOption Dubins::lsl(const RRTPoint& start, const RRTPoint& end, const XYZCoord& center_0, + const XYZCoord& center_2) { double straight_distance = center_0.distanceTo(center_2); // angle relative to horizontal @@ -241,8 +222,8 @@ RRTOption Dubins::lsl(const RRTPoint &start, const RRTPoint &end, const XYZCoord return RRTOption{total_distance, DubinsPath(beta_0, beta_2, straight_distance), true}; } -RRTOption Dubins::rsr(const RRTPoint &start, const RRTPoint &end, const XYZCoord ¢er_0, - const XYZCoord ¢er_2) const { +RRTOption Dubins::rsr(const RRTPoint& start, const RRTPoint& end, const XYZCoord& center_0, + const XYZCoord& center_2) { double straight_distance = center_0.distanceTo(center_2); // angle relative to horizontal @@ -261,8 +242,8 @@ RRTOption Dubins::rsr(const RRTPoint &start, const RRTPoint &end, const XYZCoord return RRTOption{total_distance, DubinsPath(-beta_0, -beta_2, straight_distance), true}; } -RRTOption Dubins::lsr(const RRTPoint &start, const RRTPoint &end, const XYZCoord ¢er_0, - const XYZCoord ¢er_2) const { +RRTOption Dubins::lsr(const RRTPoint& start, const RRTPoint& end, const XYZCoord& center_0, + const XYZCoord& center_2) { XYZCoord half_displacement = halfDisplacement(center_2, center_0); double psi_0 = std::atan2(half_displacement.y, half_displacement.x); double half_intercenter_distance = half_displacement.norm(); @@ -291,8 +272,8 @@ RRTOption Dubins::lsr(const RRTPoint &start, const RRTPoint &end, const XYZCoord return RRTOption{total_distance, DubinsPath(beta_0, -beta_2, straight_distance), true}; } -RRTOption Dubins::rsl(const RRTPoint &start, const RRTPoint &end, const XYZCoord ¢er_0, - const XYZCoord ¢er_2) const { +RRTOption Dubins::rsl(const RRTPoint& start, const RRTPoint& end, const XYZCoord& center_0, + const XYZCoord& center_2) { XYZCoord half_displacement = halfDisplacement(center_2, center_0); double psi_0 = std::atan2(half_displacement.y, half_displacement.x); double half_intercenter_distance = half_displacement.norm(); @@ -323,8 +304,8 @@ RRTOption Dubins::rsl(const RRTPoint &start, const RRTPoint &end, const XYZCoord return RRTOption{total_distance, DubinsPath(-beta_0, beta_2, straight_distance), true}; } -RRTOption Dubins::lrl(const RRTPoint &start, const RRTPoint &end, const XYZCoord ¢er_0, - const XYZCoord ¢er_2) const { +RRTOption Dubins::lrl(const RRTPoint& start, const RRTPoint& end, const XYZCoord& center_0, + const XYZCoord& center_2) { double intercenter_distance = center_0.distanceTo(center_2); XYZCoord half_displacement = halfDisplacement(center_2, center_0); double psi_0 = std::atan2(half_displacement.y, half_displacement.x); @@ -353,8 +334,8 @@ RRTOption Dubins::lrl(const RRTPoint &start, const RRTPoint &end, const XYZCoord return RRTOption{total_distance, DubinsPath(beta_0, beta_2, -beta_1), false}; } -RRTOption Dubins::rlr(const RRTPoint &start, const RRTPoint &end, const XYZCoord ¢er_0, - const XYZCoord ¢er_2) const { +RRTOption Dubins::rlr(const RRTPoint& start, const RRTPoint& end, const XYZCoord& center_0, + const XYZCoord& center_2) { double intercenter_distance = center_0.distanceTo(center_2); XYZCoord half_displacement = halfDisplacement(center_2, center_0); double psi_0 = std::atan2(half_displacement.y, half_displacement.x); @@ -384,8 +365,7 @@ RRTOption Dubins::rlr(const RRTPoint &start, const RRTPoint &end, const XYZCoord return RRTOption{total_distance, DubinsPath(-beta_0, -beta_2, beta_1), false}; } -std::vector Dubins::allOptions(const RRTPoint &start, const RRTPoint &end, - bool sort) const { +std::vector Dubins::allOptions(const RRTPoint& start, const RRTPoint& end) { XYZCoord center_0_left = findCenter(start, 'L'); XYZCoord center_0_right = findCenter(start, 'R'); XYZCoord center_2_left = findCenter(end, 'L'); @@ -401,21 +381,17 @@ std::vector Dubins::allOptions(const RRTPoint &start, const RRTPoint // rlr(start, end, center_0_right, center_2_right) }; - if (sort) { - std::sort(options.begin(), options.end(), compareRRTOptionLength); - } - return options; } -std::vector Dubins::dubinsPath(const RRTPoint &start, const RRTPoint &end) const { +std::vector Dubins::dubinsPath(const RRTPoint& start, const RRTPoint& end) { std::vector options = allOptions(start, end); RRTOption optimal_option = *std::min_element(options.begin(), options.end(), compareRRTOptionLength); return generatePoints(start, end, optimal_option.dubins_path, optimal_option.has_straight); } -RRTOption Dubins::bestOption(const RRTPoint &start, const RRTPoint &end) const { +RRTOption Dubins::bestOption(const RRTPoint& start, const RRTPoint& end) { std::vector options = allOptions(start, end); return *std::min_element(options.begin(), options.end(), compareRRTOptionLength); } diff --git a/src/pathing/mission_path.cpp b/src/pathing/mission_path.cpp index d89eb05c..d6e01738 100644 --- a/src/pathing/mission_path.cpp +++ b/src/pathing/mission_path.cpp @@ -45,8 +45,8 @@ void MissionPath::generateForwardCommands() { first_item.param2 = 1.0; // Accept Radius 7.0m close to 25ft first_item.param3 = 1.0; // Pass Radius first_item.param4 = NAN; // Yaw - first_item.x = int32_t(std::round(this->path.at(0).latitude() * 1e7)); - first_item.y = int32_t(std::round(this->path.at(0).longitude() * 1e7)); + first_item.x = static_cast(std::round(this->path.at(0).latitude() * 1e7)); + first_item.y = static_cast(std::round(this->path.at(0).longitude() * 1e7)); first_item.z = this->path.at(0).altitude(); first_item.mission_type = MAV_MISSION_TYPE_MISSION; this->path_mav.push_back(first_item); @@ -65,8 +65,8 @@ void MissionPath::generateForwardCommands() { item.param2 = 7.0; // Accept Radius 7.0m close to 25ft item.param3 = 0.0; // Pass Radius item.param4 = NAN; // Yaw - item.x = int32_t(std::round(coord.latitude() * 1e7)); - item.y = int32_t(std::round(coord.longitude() * 1e7)); + item.x = static_cast(std::round(coord.latitude() * 1e7)); + item.y = static_cast(std::round(coord.longitude() * 1e7)); item.z = coord.altitude(); item.mission_type = MAV_MISSION_TYPE_MISSION; this->path_mav.push_back(item); @@ -110,8 +110,8 @@ void MissionPath::generateHoverCommands() { item.param2 = 0.0f; // 0 => dont need to point heading at next waypoint item.param3 = 0.0f; // loiter radius, which shouldn't matter for hover quadplane item.param4 = 0.0f; // xtrack, something for forward flight planes which we arent here - item.x = int32_t(std::round(coord.latitude() * 1e7)); - item.y = int32_t(std::round(coord.longitude() * 1e7)); + item.x = static_cast(std::round(coord.latitude() * 1e7)); + item.y = static_cast(std::round(coord.longitude() * 1e7)); item.z = coord.altitude(); item.mission_type = MAV_MISSION_TYPE_MISSION; this->path_mav.push_back(item); @@ -141,8 +141,8 @@ void MissionPath::generateHoverAtFinalCommands() { item.param2 = 7.0; // Accept Radius 7.0m close to 25ft item.param3 = 0.0; // Pass Radius item.param4 = NAN; // Yaw - item.x = int32_t(std::round(coord.latitude() * 1e7)); - item.y = int32_t(std::round(coord.longitude() * 1e7)); + item.x = static_cast(std::round(coord.latitude() * 1e7)); + item.y = static_cast(std::round(coord.longitude() * 1e7)); item.z = coord.altitude(); item.mission_type = MAV_MISSION_TYPE_MISSION; this->path_mav.push_back(item); @@ -160,8 +160,8 @@ void MissionPath::generateHoverAtFinalCommands() { item.param2 = 0.0f; // 0 => dont need to point heading at next waypoint item.param3 = 0.0f; // loiter radius, which shouldn't matter for hover quadplane item.param4 = 0.0f; // xtrack, something for forward flight planes which we arent here - item.x = int32_t(std::round(coord.latitude() * 1e7)); - item.y = int32_t(std::round(coord.longitude() * 1e7)); + item.x = static_cast(std::round(coord.latitude() * 1e7)); + item.y = static_cast(std::round(coord.longitude() * 1e7)); item.z = coord.altitude(); item.mission_type = MAV_MISSION_TYPE_MISSION; this->path_mav.push_back(item); diff --git a/src/pathing/plotting.cpp b/src/pathing/plotting.cpp index aa6a4101..e13ac333 100644 --- a/src/pathing/plotting.cpp +++ b/src/pathing/plotting.cpp @@ -1,10 +1,12 @@ -#include -#include -#include #include +#include +#include +#include +#include +#include #include #include -#include +#include // NOTE: this is very dumb but this #include is needed // before we #include . This is because ImageMagick diff --git a/src/pathing/static.cpp b/src/pathing/static.cpp index 2039251d..4ede066f 100644 --- a/src/pathing/static.cpp +++ b/src/pathing/static.cpp @@ -25,8 +25,7 @@ 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(config.pathing.rrt) { if (angles.size() != 0) { this->angles = angles; @@ -38,8 +37,7 @@ RRT::RRT(RRTPoint start, std::vector goals, double search_radius, Envi : iterations_per_waypoint(config.pathing.rrt.iterations_per_waypoint), search_radius(search_radius), rewire_radius(config.pathing.rrt.rewire_radius), - tree(start, airspace, - Dubins(config.pathing.dubins.turning_radius, config.pathing.dubins.point_separation)), + tree(start, airspace), config(config.pathing.rrt) { if (angles.size() != 0) { this->angles = angles; @@ -303,7 +301,6 @@ ForwardCoveragePathing::ForwardCoveragePathing(const RRTPoint& start, double sca : start(start), scan_radius(scan_radius), airspace(Environment(bounds, airdrop_zone, {}, {}, obstacles)), - dubins(Dubins(config.pathing.dubins.turning_radius, config.pathing.dubins.point_separation)), config(config.pathing.coverage) {} std::vector ForwardCoveragePathing::run() const { @@ -320,7 +317,7 @@ std::vector ForwardCoveragePathing::coverageDefault() const { // generates the path connecting the q std::vector dubins_options; for (int i = 0; i < waypoints.size() - 1; i++) { - dubins_options.push_back(dubins.bestOption(waypoints[i], waypoints[i + 1])); + dubins_options.push_back(Dubins::bestOption(waypoints[i], waypoints[i + 1])); } return generatePath(dubins_options, waypoints); @@ -352,7 +349,7 @@ std::vector ForwardCoveragePathing::coverageOptimal() const { std::vector current_dubins_path; for (int i = 0; i < waypoints.size() - 1; i++) { - RRTOption dubins_path = dubins.bestOption(waypoints[i], waypoints[i + 1]); + RRTOption dubins_path = Dubins::bestOption(waypoints[i], waypoints[i + 1]); lengths[i] += dubins_path.length; current_dubins_path.push_back(dubins_path); } @@ -387,7 +384,7 @@ std::vector ForwardCoveragePathing::generatePath( double height = waypoints[0].coord.z; double height_difference = config.altitude_m - waypoints[0].coord.z; - std::vector path_coordinates = dubins.generatePoints( + std::vector path_coordinates = Dubins::generatePoints( waypoints[0], waypoints[1], dubins_options[0].dubins_path, dubins_options[0].has_straight); double height_increment = height_difference / path_coordinates.size(); @@ -402,7 +399,7 @@ std::vector ForwardCoveragePathing::generatePath( // main loop for (int i = 1; i < dubins_options.size(); i++) { path_coordinates = - dubins.generatePoints(waypoints[i], waypoints[i + 1], dubins_options[i].dubins_path, + Dubins::generatePoints(waypoints[i], waypoints[i + 1], dubins_options[i].dubins_path, dubins_options[i].has_straight); for (XYZCoord& coord : path_coordinates) { @@ -511,7 +508,6 @@ AirdropApproachPathing::AirdropApproachPathing(const RRTPoint& start, const XYZC goal(goal), wind(wind), airspace(Environment(bounds, {}, {}, {goal}, obstacles)), - dubins(Dubins(config.pathing.dubins.turning_radius, config.pathing.dubins.point_separation)), config(config) {} std::vector AirdropApproachPathing::run() const { diff --git a/src/pathing/tree.cpp b/src/pathing/tree.cpp index 4373e819..25695628 100644 --- a/src/pathing/tree.cpp +++ b/src/pathing/tree.cpp @@ -1,5 +1,8 @@ #include "pathing/tree.hpp" +#include +#include +#include #include #include #include @@ -71,8 +74,8 @@ void RRTNode::setPathLength(double new_path_length) { this->path_length = new_pa */ -RRTTree::RRTTree(RRTPoint root_point, Environment airspace, Dubins dubins) - : airspace(airspace), dubins(dubins), tree_size(1) { +RRTTree::RRTTree(RRTPoint root_point, Environment airspace) + : airspace(airspace), tree_size(1) { std::shared_ptr new_node = std::make_shared(root_point, 0, 0, std::vector{}); root = new_node; @@ -90,7 +93,7 @@ bool RRTTree::validatePath(const std::vector& path, const RRTOption& o std::shared_ptr RRTTree::generateNode(std::shared_ptr anchor_node, const RRTPoint& new_point, const RRTOption& option) const { - const std::vector& path = dubins.generatePoints( + const std::vector& path = Dubins::generatePoints( anchor_node->getPoint(), new_point, option.dubins_path, option.has_straight); if (!validatePath(path, option)) { @@ -217,7 +220,7 @@ void RRTTree::fillOptionsNodes(std::vector, R const std::vector>& nodes, const RRTPoint& sample) const { for (std::shared_ptr node : nodes) { - const std::vector& local_options = dubins.allOptions(node->getPoint(), sample); + const std::vector& local_options = Dubins::allOptions(node->getPoint(), sample); for (const RRTOption& option : local_options) { if (std::isnan(option.length) || @@ -306,7 +309,7 @@ void RRTTree::fillOptions(std::vector, RRTOpt } // gets all dubins curves from the current node to the end point - const std::vector& local_options = dubins.allOptions(node->getPoint(), end); + const std::vector& local_options = Dubins::allOptions(node->getPoint(), end); // filters out the options that are not valid for (const RRTOption& option : local_options) { @@ -435,8 +438,10 @@ void RRTTree::RRTStarRecursive(std::shared_ptr current_node, } // get the dubins options (sorted) - const std::vector& options = - dubins.allOptions(sample->getPoint(), child->getPoint(), true); + std::vector options = + Dubins::allOptions(sample->getPoint(), child->getPoint()); + std::sort(options.begin(), options.end(), compareRRTOptionLength); + // for each option for (const RRTOption& option : options) { @@ -455,8 +460,11 @@ void RRTTree::RRTStarRecursive(std::shared_ptr current_node, // if the new cost is less than the current cost // check if new path is valid - const std::vector& path = dubins.generatePoints( - sample->getPoint(), child->getPoint(), option.dubins_path, option.has_straight); + const std::vector& path = + Dubins::generatePoints(sample->getPoint(), + child->getPoint(), + option.dubins_path, + option.has_straight); if (!validatePath(path, option)) { continue; diff --git a/src/ticks/manual_landing.cpp b/src/ticks/manual_landing.cpp index 110a3aa0..4a3ffab7 100644 --- a/src/ticks/manual_landing.cpp +++ b/src/ticks/manual_landing.cpp @@ -1,6 +1,7 @@ #include "ticks/manual_landing.hpp" #include +#include #include "cv/mapping.hpp" #include "ticks/ids.hpp" diff --git a/src/ticks/mission_prep.cpp b/src/ticks/mission_prep.cpp index 3ffd6ef4..433a6e47 100644 --- a/src/ticks/mission_prep.cpp +++ b/src/ticks/mission_prep.cpp @@ -4,6 +4,7 @@ #include #include +#include #include "core/mission_state.hpp" #include "ticks/ids.hpp" diff --git a/src/utilities/datatypes.cpp b/src/utilities/datatypes.cpp index e7993b97..8c93d195 100644 --- a/src/utilities/datatypes.cpp +++ b/src/utilities/datatypes.cpp @@ -1,6 +1,8 @@ #include "utilities/datatypes.hpp" #include +#include +#include #include "pathing/cartesian.hpp" #include "protos/obc.pb.h" diff --git a/src/utilities/obc_config.cpp b/src/utilities/obc_config.cpp index aeec5f6b..f4b4c3f8 100644 --- a/src/utilities/obc_config.cpp +++ b/src/utilities/obc_config.cpp @@ -6,6 +6,7 @@ #include #include "nlohmann/json.hpp" +#include "pathing/dubins.hpp" #include "udp_squared/internal/enum.h" #include "utilities/constants.hpp" #include "utilities/datatypes.hpp" @@ -128,4 +129,7 @@ OBCConfig::OBCConfig(int argc, char* argv[]) { // were previously set in the common file this->mavlink_parameters.param_map[param] = val; } + + Dubins::_radius = this->pathing.dubins.turning_radius; + Dubins::_point_separation = this->pathing.dubins.point_separation; } diff --git a/tests/unit/pathing/dubins_test.cpp b/tests/unit/pathing/dubins_test.cpp index 1086ad70..95ef87ff 100644 --- a/tests/unit/pathing/dubins_test.cpp +++ b/tests/unit/pathing/dubins_test.cpp @@ -8,6 +8,11 @@ typedef XYZCoord Vector; +static inline void setDubins(double r, double sep) { + Dubins::_radius = r; + Dubins::_point_separation = sep; +} + /* * NOTE: the use of () and {} constructors is non-staandard * i.e. I originally wrote it using () and was too lazy to @@ -118,27 +123,27 @@ TEST(DubinsUtilTest, HalfDisplacement) { * tests Dubins::findCenter() */ TEST(DubinsTest, FindCenter) { - Dubins dubins1(5, 10); + setDubins(5, 10); // points towards e1 RRTPoint origin_x{Vector{0, 0, 0}, 0}; - Vector result1 = dubins1.findCenter(origin_x, 'L'); + Vector result1 = Dubins::findCenter(origin_x, 'L'); Vector expected_result1{0.0, 5.0, 0}; - Vector result2 = dubins1.findCenter(origin_x, 'R'); + Vector result2 = Dubins::findCenter(origin_x, 'R'); Vector expected_result2{0.0, -5.0, 0}; // points towards e2 RRTPoint origin_y{Vector{0, 0, 0}, M_PI / 2}; - Vector result3 = dubins1.findCenter(origin_y, 'L'); + Vector result3 = Dubins::findCenter(origin_y, 'L'); Vector expected_result3{-5.0, 0.0, 0}; - Vector result4 = dubins1.findCenter(origin_y, 'R'); + Vector result4 = Dubins::findCenter(origin_y, 'R'); Vector expected_result4{5.0, 0.0, 0}; RRTPoint arbitrary{Vector{12, 156, 100}, 1.3}; - Vector result5 = dubins1.findCenter(arbitrary, 'L'); + Vector result5 = Dubins::findCenter(arbitrary, 'L'); // [-4.817, 1.341] ==> magnitude 5 * e1 vector rotated 2.87 [1.3 + pi/2] raidans Vector expected_result5{12 - 4.817, 156 + 1.341, 0}; - Vector result6 = dubins1.findCenter(arbitrary, 'R'); + Vector result6 = Dubins::findCenter(arbitrary, 'R'); Vector expected_result6{12 + 4.817, 156 - 1.341, 0}; EXPECT_NEAR(result1.x, expected_result1.x, 0.01); @@ -166,7 +171,7 @@ TEST(DubinsTest, FindCenter) { * */ TEST(DubinsTest, CircleArc) { - Dubins dubins1(5, 10); + setDubins(5, 10); // points towards e1 RRTPoint origin_x{Vector{0, 0, 0}, 0}; @@ -176,19 +181,19 @@ TEST(DubinsTest, CircleArc) { // plane is facing x+, turning left/ccw with a turning radius of 5, // this should be the point where it turns 90deg (1/4 of the circle) Vector result1 = - dubins1.circleArc(origin_x, 1, dubins1.findCenter(origin_x, 'L'), M_PI / 2 * 5); + Dubins::circleArc(origin_x, 1, Dubins::findCenter(origin_x, 'L'), M_PI / 2 * 5); Vector expected_result1{5.0, 5.0, 0}; // plance facing x+, turning right/cw with a turning radius of 5 // turning 2.97 rad - Vector result2 = dubins1.circleArc(origin_x, -1, dubins1.findCenter(origin_x, 'R'), 2.97 * 5); + Vector result2 = Dubins::circleArc(origin_x, -1, Dubins::findCenter(origin_x, 'R'), 2.97 * 5); Vector expected_result2{0.850, -9.927, 0}; - Vector result3 = dubins1.circleArc(arbitrary_position1, 1, - dubins1.findCenter(arbitrary_position1, 'L'), 5.12 * 5); + Vector result3 = Dubins::circleArc(arbitrary_position1, 1, + Dubins::findCenter(arbitrary_position1, 'L'), 5.12 * 5); Vector expected_result3{78.28441936, 42.50134993, 0}; - Vector result4 = dubins1.circleArc(origin_y, 1, dubins1.findCenter(origin_y, 'L'), M_PI * 5); + Vector result4 = Dubins::circleArc(origin_y, 1, Dubins::findCenter(origin_y, 'L'), M_PI * 5); Vector expected_result4{-10.0, 0.0, 0}; EXPECT_NEAR(result1.x, expected_result1.x, 0.01); @@ -209,7 +214,7 @@ TEST(DubinsTest, CircleArc) { * fails at last turn */ TEST(DubinsTest, GenPointsStraight) { - Dubins dubins1{5, 1}; + setDubins(5, 1); // points towards e1 RRTPoint origin_x{Vector{0, 0, 0}, 0}; RRTPoint arbitrary_position1{Vector{9, 6, 0}, 4.00}; @@ -218,7 +223,7 @@ TEST(DubinsTest, GenPointsStraight) { DubinsPath path{6.107586558274035, 4.175598748905551, 12.983673916464376}; std::vector result1 = - dubins1.generatePointsStraight(origin_x, arbitrary_position1, path); + Dubins::generatePointsStraight(origin_x, arbitrary_position1, path); std::vector expected_result1 = {Vector{6.123233995736766e-16, 0.0, 0}, Vector{0.9933466539753065, 0.09966711079379209, 0}, Vector{1.9470917115432524, 0.3946950299855745, 0}, @@ -299,7 +304,7 @@ TEST(DubinsTest, GenPointsStraight) { * tests Dubins::generatePointsCurve() */ TEST(DubinsTest, GenPointsCurve) { - Dubins dubins1{5, 1}; + setDubins(5, 1); // points towards e1 RRTPoint origin_x{Vector{0, 0, 0}, 0}; RRTPoint arbitrary_position1{Vector{9, 6, 0}, 4.00}; @@ -307,7 +312,7 @@ TEST(DubinsTest, GenPointsCurve) { // lrl origin_x ==> arbitrary_position DubinsPath path{2.25948315258286, 0.3274953432143759, 4.870163802976823}; - std::vector result1 = dubins1.generatePointsCurve(origin_x, arbitrary_position1, path); + std::vector result1 = Dubins::generatePointsCurve(origin_x, arbitrary_position1, path); std::vector expected_result1 = {Vector{6.123233995736766e-16, 0.0, 0}, Vector{0.9933466539753065, 0.09966711079379209, 0}, Vector{1.9470917115432524, 0.3946950299855745, 0}, @@ -361,7 +366,7 @@ TEST(DubinsTest, GenPointsCurve) { * tests Dubins::generatePoints() */ TEST(DubinsTest, GenPoints) { - Dubins dubins1{5, 1}; + setDubins(5, 1); // points towards e1 RRTPoint origin_x{Vector{0, 0, 0}, 0}; RRTPoint arbitrary_position1{Vector{9, 6, 0}, 4.00}; @@ -374,9 +379,9 @@ TEST(DubinsTest, GenPoints) { DubinsPath{2.25948315258286, 0.3274953432143759, -4.870163802976823}, false}; std::vector result1 = - dubins1.generatePoints(origin_x, arbitrary_position1, lsl.dubins_path, lsl.has_straight); + Dubins::generatePoints(origin_x, arbitrary_position1, lsl.dubins_path, lsl.has_straight); std::vector result2 = - dubins1.generatePoints(origin_x, arbitrary_position1, lrl.dubins_path, lrl.has_straight); + Dubins::generatePoints(origin_x, arbitrary_position1, lrl.dubins_path, lrl.has_straight); std::vector expected_result1 = {Vector{6.123233995736766e-16, 0.0, 0}, Vector{0.9933466539753065, 0.09966711079379209, 0}, @@ -505,7 +510,7 @@ TEST(DubinsTest, GenPoints) { * tests Dubins::lsl() */ TEST(DubinsTest, LSL) { - Dubins dubins1(5, 10); + setDubins(5, 10); // points towards e1 RRTPoint origin_x{Vector{0, 0, 0}, 0}; @@ -514,8 +519,8 @@ TEST(DubinsTest, LSL) { RRTPoint arbitrary_position2{Vector{5, 100, 0}, M_PI / 2}; RRTOption result1 = - dubins1.lsl(origin_x, arbitrary_position1, dubins1.findCenter(origin_x, 'L'), - dubins1.findCenter(arbitrary_position1, 'L')); + Dubins::lsl(origin_x, arbitrary_position1, Dubins::findCenter(origin_x, 'L'), + Dubins::findCenter(arbitrary_position1, 'L')); RRTOption expected_result1{103.46948015930067, DubinsPath{0.40295754, 3.5970424510, 83.46948015930067}, true}; @@ -525,8 +530,8 @@ TEST(DubinsTest, LSL) { EXPECT_NEAR(result1.dubins_path.straight_dist, result1.dubins_path.straight_dist, 0.01); EXPECT_EQ(result1.has_straight, expected_result1.has_straight); - RRTOption result2 = dubins1.lsl(origin_x, plus_x100, dubins1.findCenter(origin_x, 'L'), - dubins1.findCenter(plus_x100, 'L')); + RRTOption result2 = Dubins::lsl(origin_x, plus_x100, Dubins::findCenter(origin_x, 'L'), + Dubins::findCenter(plus_x100, 'L')); RRTOption expected_result2{100, DubinsPath{0, 0, 100}, true}; EXPECT_NEAR(result2.length, expected_result2.length, 0.01); @@ -536,8 +541,8 @@ TEST(DubinsTest, LSL) { EXPECT_EQ(result2.has_straight, expected_result2.has_straight); RRTOption result3 = - dubins1.lsl(origin_x, arbitrary_position2, dubins1.findCenter(origin_x, 'L'), - dubins1.findCenter(arbitrary_position2, 'L')); + Dubins::lsl(origin_x, arbitrary_position2, Dubins::findCenter(origin_x, 'L'), + Dubins::findCenter(arbitrary_position2, 'L')); RRTOption expected_result3{102.85398163397448, DubinsPath{M_PI / 2, 0, 95}, true}; EXPECT_NEAR(result3.length, expected_result3.length, 0.01); @@ -551,7 +556,7 @@ TEST(DubinsTest, LSL) { * tests Dubins::rsr() */ TEST(DubinsTest, RSR) { - Dubins dubins1(5, 10); + setDubins(5, 10); // points towards e1 RRTPoint origin_x{Vector{0, 0, 0}, 0}; RRTPoint arbitrary_position1{Vector{73, 41, 0}, 4.00}; @@ -559,8 +564,8 @@ TEST(DubinsTest, RSR) { RRTPoint arbitrary_position2{Vector{5, -100, 0}, -M_PI / 2}; RRTOption result1 = - dubins1.rsr(origin_x, arbitrary_position1, dubins1.findCenter(origin_x, 'R'), - dubins1.findCenter(arbitrary_position1, 'R')); + Dubins::rsr(origin_x, arbitrary_position1, Dubins::findCenter(origin_x, 'R'), + Dubins::findCenter(arbitrary_position1, 'R')); RRTOption expected_result1( 127.792, DubinsPath(-5.664581035483313, -2.9017895788758596, 84.96005087111514), true); @@ -570,8 +575,8 @@ TEST(DubinsTest, RSR) { EXPECT_NEAR(result1.dubins_path.straight_dist, result1.dubins_path.straight_dist, 0.01); EXPECT_EQ(result1.has_straight, expected_result1.has_straight); - RRTOption result2 = dubins1.rsr(origin_x, plus_x100, dubins1.findCenter(origin_x, 'R'), - dubins1.findCenter(plus_x100, 'R')); + RRTOption result2 = Dubins::rsr(origin_x, plus_x100, Dubins::findCenter(origin_x, 'R'), + Dubins::findCenter(plus_x100, 'R')); RRTOption expected_result2(100, DubinsPath(0, 0, 100), true); EXPECT_NEAR(result2.length, expected_result2.length, 0.01); @@ -581,8 +586,8 @@ TEST(DubinsTest, RSR) { EXPECT_EQ(result2.has_straight, expected_result2.has_straight); RRTOption result3 = - dubins1.rsr(origin_x, arbitrary_position2, dubins1.findCenter(origin_x, 'R'), - dubins1.findCenter(arbitrary_position2, 'R')); + Dubins::rsr(origin_x, arbitrary_position2, Dubins::findCenter(origin_x, 'R'), + Dubins::findCenter(arbitrary_position2, 'R')); RRTOption expected_result3(102.85398163397448, DubinsPath(-M_PI / 2, 0, 95), true); EXPECT_NEAR(result3.length, expected_result3.length, 0.01); @@ -596,7 +601,7 @@ TEST(DubinsTest, RSR) { * tests Dubins::rsl() */ TEST(DubinsTest, RSL) { - Dubins dubins1(5, 10); + setDubins(5, 10); // points towards e1 RRTPoint origin_x{Vector{0, 0, 0}, 0}; RRTPoint arbitrary_position1{Vector{73, 41, 0}, 4.00}; @@ -604,8 +609,8 @@ TEST(DubinsTest, RSL) { RRTPoint arbitrary_position2{Vector{10, -100, 0}, 0}; RRTOption result1 = - dubins1.rsl(origin_x, arbitrary_position1, dubins1.findCenter(origin_x, 'R'), - dubins1.findCenter(arbitrary_position1, 'L')); + Dubins::rsl(origin_x, arbitrary_position1, Dubins::findCenter(origin_x, 'R'), + Dubins::findCenter(arbitrary_position1, 'L')); RRTOption expected_result1( 134.78090998278276, DubinsPath(-5.8893974274779834, 3.606212120298397, 87.30286224390085), true); @@ -616,8 +621,8 @@ TEST(DubinsTest, RSL) { EXPECT_NEAR(result1.dubins_path.straight_dist, result1.dubins_path.straight_dist, 0.01); EXPECT_EQ(result1.has_straight, expected_result1.has_straight); - RRTOption result2 = dubins1.rsl(origin_x, plus_x100, dubins1.findCenter(origin_x, 'R'), - dubins1.findCenter(plus_x100, 'L')); + RRTOption result2 = Dubins::rsl(origin_x, plus_x100, Dubins::findCenter(origin_x, 'R'), + Dubins::findCenter(plus_x100, 'L')); RRTOption expected_result2(100, DubinsPath(0, 0, 100), true); EXPECT_NEAR(result2.length, expected_result2.length, 0.01); @@ -627,8 +632,8 @@ TEST(DubinsTest, RSL) { EXPECT_EQ(result2.has_straight, expected_result2.has_straight); RRTOption result3 = - dubins1.rsl(origin_x, arbitrary_position2, dubins1.findCenter(origin_x, 'R'), - dubins1.findCenter(arbitrary_position2, 'L')); + Dubins::rsl(origin_x, arbitrary_position2, Dubins::findCenter(origin_x, 'R'), + Dubins::findCenter(arbitrary_position2, 'L')); RRTOption expected_result3(105.70796326794898, DubinsPath(-M_PI / 2, M_PI / 2, 90), true); EXPECT_NEAR(result3.length, expected_result3.length, 0.01); @@ -642,7 +647,7 @@ TEST(DubinsTest, RSL) { * tests Dubins::lsr() */ TEST(DubinsTest, LSR) { - Dubins dubins1(5, 10); + setDubins(5, 10); // points towards e1 RRTPoint origin_x{Vector{0, 0, 0}, 0}; RRTPoint arbitrary_position1{Vector{73, 41, 0}, 4.00}; @@ -650,8 +655,8 @@ TEST(DubinsTest, LSR) { RRTPoint arbitrary_position2{Vector{10, 100, 0}, 0}; RRTOption result1 = - dubins1.lsr(origin_x, arbitrary_position1, dubins1.findCenter(origin_x, 'L'), - dubins1.findCenter(arbitrary_position1, 'R')); + Dubins::lsr(origin_x, arbitrary_position1, Dubins::findCenter(origin_x, 'L'), + Dubins::findCenter(arbitrary_position1, 'R')); RRTOption expected_result1( 96.78474229907584, DubinsPath(0.6420440973470476, -2.925229404526634, 78.94837478970744), true); @@ -662,8 +667,8 @@ TEST(DubinsTest, LSR) { EXPECT_NEAR(result1.dubins_path.straight_dist, result1.dubins_path.straight_dist, 0.01); EXPECT_EQ(result1.has_straight, expected_result1.has_straight); - RRTOption result2 = dubins1.lsr(origin_x, plus_x100, dubins1.findCenter(origin_x, 'L'), - dubins1.findCenter(plus_x100, 'R')); + RRTOption result2 = Dubins::lsr(origin_x, plus_x100, Dubins::findCenter(origin_x, 'L'), + Dubins::findCenter(plus_x100, 'R')); RRTOption expected_result2(100, DubinsPath(0, 0, 100), true); EXPECT_NEAR(result2.length, expected_result2.length, 0.01); @@ -673,8 +678,8 @@ TEST(DubinsTest, LSR) { EXPECT_EQ(result2.has_straight, expected_result2.has_straight); RRTOption result3 = - dubins1.lsr(origin_x, arbitrary_position2, dubins1.findCenter(origin_x, 'L'), - dubins1.findCenter(arbitrary_position2, 'R')); + Dubins::lsr(origin_x, arbitrary_position2, Dubins::findCenter(origin_x, 'L'), + Dubins::findCenter(arbitrary_position2, 'R')); RRTOption expected_result3(105.70796326794898, DubinsPath(M_PI / 2, -M_PI / 2, 90), true); EXPECT_NEAR(result3.length, expected_result3.length, 0.01); EXPECT_NEAR(result3.dubins_path.beta_0, expected_result3.dubins_path.beta_0, 0.01); @@ -687,14 +692,14 @@ TEST(DubinsTest, LSR) { * tests Dubins::lrl() */ TEST(DubinsTest, LRL) { - Dubins dubins1(5, 10); + setDubins(5, 10); // points towards e1 RRTPoint origin_x{Vector{0, 0, 0}, 0}; RRTPoint arbitrary_position1{Vector{9, 6, 0}, 4.00}; RRTOption result1 = - dubins1.lrl(origin_x, arbitrary_position1, dubins1.findCenter(origin_x, 'L'), - dubins1.findCenter(arbitrary_position1, 'L')); + Dubins::lrl(origin_x, arbitrary_position1, Dubins::findCenter(origin_x, 'L'), + Dubins::findCenter(arbitrary_position1, 'L')); RRTOption expected_result1(37.28571149387029, DubinsPath(2.25948315258286, 0.3274953432143759, 4.870163802976823), false); @@ -710,14 +715,14 @@ TEST(DubinsTest, LRL) { * tests Dubins::rlr() */ TEST(DubinsTest, RLR) { - Dubins dubins1(5, 10); + setDubins(5, 10); // points towards e1 RRTPoint origin_x{Vector{0, 0, 0}, 0}; RRTPoint arbitrary_position1{Vector{9, 6, 0}, 4.00}; RRTOption result1 = - dubins1.rlr(origin_x, arbitrary_position1, dubins1.findCenter(origin_x, 'R'), - dubins1.findCenter(arbitrary_position1, 'R')); + Dubins::rlr(origin_x, arbitrary_position1, Dubins::findCenter(origin_x, 'R'), + Dubins::findCenter(arbitrary_position1, 'R')); RRTOption expected_result1( 56.99424154724155, DubinsPath(-1.0585943958426456, -5.782422412471302, 4.557831501134362), false); @@ -733,13 +738,13 @@ TEST(DubinsTest, RLR) { * tests Dubins::allOptions() */ TEST(DubinsTest, AllOptions) { - Dubins dubins1(5, 10); + setDubins(5, 10); // points towards e1 RRTPoint origin_x{Vector{0, 0, 0}, 0}; RRTPoint arbitrary_position1{Vector{9, 6, 0}, 4.00}; RRTPoint arbitrary_position2{Vector{3, -1, 0}, 2.36}; - std::vector result1 = dubins1.allOptions(origin_x, arbitrary_position1); + std::vector result1 = Dubins::allOptions(origin_x, arbitrary_position1); std::vector expected_result1 = { RRTOption(64.39960045236231, DubinsPath(6.107586558274035, 4.175598748905551, 12.983673916464376), true), @@ -754,7 +759,7 @@ TEST(DubinsTest, AllOptions) { DubinsPath(2.25948315258286, 0.3274953432143759, -4.870163802976823), false), }; - std::vector result2 = dubins1.allOptions(arbitrary_position1, arbitrary_position2); + std::vector result2 = Dubins::allOptions(arbitrary_position1, arbitrary_position2); std::vector expected_result2 = { RRTOption(69.79960318782443, DubinsPath(5.92544955425334, 5.000921060105833, 15.167750116028579), true), @@ -811,12 +816,12 @@ TEST(DubinsTest, AllOptions) { * tests Dubins::dubinsPath() */ TEST(DubinsTest, DubinsPath) { - Dubins dubins1{5, 1}; + setDubins(5, 1); // points towards e1 RRTPoint origin_x{Vector{0, 0, 0}, 0}; RRTPoint arbitrary_position1{Vector{9, 6, 0}, 4.00}; - std::vector result1 = dubins1.dubinsPath(origin_x, arbitrary_position1); + std::vector result1 = Dubins::dubinsPath(origin_x, arbitrary_position1); std::vector expected_result1 = {Vector{6.123233995736766e-16, 0.0, 0}, Vector{0.993400836368525, -0.09938973742323216, 0}, Vector{1.975524298544812, -0.2876276322341959, 0}, diff --git a/tests/unit/pathing/tree_test.cpp b/tests/unit/pathing/tree_test.cpp index e491c576..84f08180 100644 --- a/tests/unit/pathing/tree_test.cpp +++ b/tests/unit/pathing/tree_test.cpp @@ -13,8 +13,13 @@ */ #include +static inline void setDubins(double r, double sep) { + Dubins::_radius = r; + Dubins::_point_separation = sep; +} + TEST(SimpleTreeTest, addNodeTest) { - Dubins dubins{5, 0.1}; + setDubins(5, 0.1); Polygon valid_region; valid_region.emplace_back(XYZCoord(0, 0, 0)); valid_region.emplace_back(XYZCoord(100, 0, 0)); @@ -28,9 +33,9 @@ TEST(SimpleTreeTest, addNodeTest) { 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]; + RRTOption option = Dubins::bestOption(point1, point2); - RRTTree simple_tree = RRTTree(point1, env, dubins); + RRTTree simple_tree = RRTTree(point1, env); std::shared_ptr root = simple_tree.getRoot(); @@ -43,7 +48,7 @@ TEST(SimpleTreeTest, addNodeTest) { } TEST(SimpleTreeTest, rewireEdgeTest) { - Dubins dubins{5, 0.1}; + setDubins(5, 0.1); Polygon valid_region; valid_region.emplace_back(XYZCoord(0, 0, 0)); valid_region.emplace_back(XYZCoord(100, 0, 0)); @@ -59,12 +64,12 @@ TEST(SimpleTreeTest, rewireEdgeTest) { RRTPoint point3 = RRTPoint(XYZCoord(50, 80, 1.5), HALF_PI); RRTPoint point4 = RRTPoint(XYZCoord(50, 60, 0.9), HALF_PI); - RRTOption option1 = dubins.allOptions(point1, point2, true)[0]; - RRTOption option2 = dubins.allOptions(point2, point3, true)[0]; - RRTOption option3 = dubins.allOptions(point1, point4, true)[0]; - RRTOption new_option = dubins.allOptions(point4, point3, true)[0]; + RRTOption option1 = Dubins::allOptions(point1, point2)[0]; + RRTOption option2 = Dubins::allOptions(point2, point3)[0]; + RRTOption option3 = Dubins::allOptions(point1, point4)[0]; + RRTOption new_option = Dubins::allOptions(point4, point3)[0]; - RRTTree simple_tree = RRTTree(point1, env, dubins); + RRTTree simple_tree = RRTTree(point1, env); std::shared_ptr root = simple_tree.getRoot(); diff --git a/tests/unit/pathing_test.cpp b/tests/unit/pathing_test.cpp index 9cc999e9..3cc2525e 100644 --- a/tests/unit/pathing_test.cpp +++ b/tests/unit/pathing_test.cpp @@ -24,6 +24,11 @@ #include "handler_params.hpp" +static inline void setDubins(double r, double sep) { + Dubins::_radius = r; + Dubins::_point_separation = sep; +} + // TODO: fails fom the tick ever switching // copies over code verbatim from the gcs test, and then generates a path // TEST(StaticPathingTest, RRTTest) { @@ -184,12 +189,12 @@ TEST(StaticPathingTest, FinalAngleDubinsPath) { const double EXPECTED_END_ANGLE = M_PI / 2.0; - Dubins dubins(30.0, 30.0); + setDubins(30.0, 30.0); RRTPoint start_pt(XYZCoord(0.0, 0.0, 0.0), 0.0); RRTPoint end_pt(XYZCoord(300.0, 300.0, 0.0), EXPECTED_END_ANGLE); // Dubins Path - std::vector dubins_xyz = dubins.dubinsPath(start_pt, end_pt); + std::vector dubins_xyz = Dubins::dubinsPath(start_pt, end_pt); std::vector dubins_gps; for (const auto& xyz : dubins_xyz) { dubins_gps.push_back(converter.toLatLng(xyz));