From da93cdd1585507c0331d6707e6a6f6695fac8e15 Mon Sep 17 00:00:00 2001 From: AskewParity Date: Mon, 27 Jul 2026 22:52:25 -0700 Subject: [PATCH 1/2] chore: flatten dubins --- include/pathing/dubins.hpp | 88 ++-- include/pathing/static.hpp | 2 - include/pathing/tree.hpp | 3 +- src/pathing/dubins.cpp | 654 ++++++++++++++--------------- src/pathing/static.cpp | 16 +- src/pathing/tree.cpp | 24 +- src/utilities/obc_config.cpp | 4 + tests/unit/pathing/dubins_test.cpp | 123 +++--- tests/unit/pathing/tree_test.cpp | 23 +- tests/unit/pathing_test.cpp | 9 +- 10 files changed, 479 insertions(+), 467 deletions(-) diff --git a/include/pathing/dubins.hpp b/include/pathing/dubins.hpp index 53f7ecce..24108c56 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,11 +83,11 @@ 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 @@ -97,7 +97,7 @@ class Dubins { * @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; + XYZCoord findCenter(const RRTPoint& point, char side); /** * Finds a point (vector) along a curved path given a distance @@ -110,8 +110,10 @@ class Dubins { * @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; + 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] @@ -121,8 +123,9 @@ class Dubins { * @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; + std::vector generatePointsStraight(const RRTPoint& start, + const RRTPoint& end, + const DubinsPath& path); /** * Generates points for the paths that only contain curved sections @@ -133,8 +136,9 @@ class Dubins { * @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; + std::vector generatePointsCurve(const RRTPoint& start, + const RRTPoint& end, + const DubinsPath& path); /** * Abstraction for generating points (curved/straight) @@ -146,8 +150,10 @@ class Dubins { * @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; + 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 @@ -167,8 +173,10 @@ class Dubins { * - 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; + 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 @@ -187,8 +195,10 @@ class Dubins { * - 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; + 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 @@ -210,8 +220,10 @@ class Dubins { * - 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; + 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 @@ -233,8 +245,10 @@ class Dubins { * - 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; + 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, @@ -252,8 +266,10 @@ class Dubins { * - 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; + 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, @@ -271,8 +287,10 @@ class Dubins { * - 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; + 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 @@ -280,13 +298,11 @@ class Dubins { * * @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; + std::vector allOptions(const RRTPoint& start, + const RRTPoint& end); /** * Compute all the possible Dubin's path(s) and @@ -296,8 +312,9 @@ class Dubins { * @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; - + std::vector dubinsPath(const RRTPoint& start, + const RRTPoint& end); + /** * Returns the optimal RRTOption from the list of options * @@ -305,11 +322,8 @@ class Dubins { * @param end ==> vector at end position * @return ==> the optimal RRTOption from the list of options */ - RRTOption bestOption(const RRTPoint &start, const RRTPoint &end) const; + RRTOption bestOption(const RRTPoint& start, const RRTPoint& end); - private: - const double _radius; - const double _point_separation; }; #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/src/pathing/dubins.cpp b/src/pathing/dubins.cpp index 84286921..e7c1dac0 100644 --- a/src/pathing/dubins.cpp +++ b/src/pathing/dubins.cpp @@ -6,6 +6,8 @@ #include "utilities/datatypes.hpp" +using namespace Dubins; + /** * Notes from Christopher: * @@ -31,391 +33,369 @@ * if right turn ==> -beta) */ + enum turn { + FIRST_TURN, + MIDDLE_TURN, + LAST_TURN + }; + template int sign(T number) { - return (T(0) < number) - (number < T(0)); + 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); -} - -bool compareRRTOptionLength(const RRTOption &first, const RRTOption &second) { - return first.length < second.length; + return std::fmod(std::fmod(dividend, divisor) + divisor, divisor); } -XYZCoord findOrthogonalVector2D(const XYZCoord &vector) { - return XYZCoord{-vector.y, vector.x, vector.z}; +bool compareRRTOptionLength(const RRTOption& first, const RRTOption& second) { + return first.length < second.length; } -XYZCoord halfDisplacement(const XYZCoord &vector1, const XYZCoord &vector2) { - return 0.5 * (vector1 - vector2); +XYZCoord findOrthogonalVector2D(const XYZCoord& vector) { + return XYZCoord{-vector.y, vector.x, vector.z}; } -Dubins::Dubins(double radius, double point_separation) - : _radius(radius), _point_separation(point_separation) { - assert(radius > 0); - assert(point_separation > 0); +XYZCoord halfDisplacement(const XYZCoord& vector1, const XYZCoord& vector2) { + return 0.5 * (vector1 - vector2); } -XYZCoord Dubins::findCenter(const RRTPoint &point, char side) const { - assert(side == 'L' || side == 'R'); +XYZCoord Dubins::findCenter(const RRTPoint& point, char side) { + assert(side == 'L' || side == 'R'); - // creates a right angle between the RRTPoint vector towards the center - // left is 90 deg CCW, right is 90 deg CW - double angle = point.psi + (side == 'L' ? HALF_PI : -HALF_PI); + // creates a right angle between the RRTPoint vector towards the center + // left is 90 deg CCW, right is 90 deg CW + double angle = point.psi + (side == 'L' ? HALF_PI : -HALF_PI); - // 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}; + // 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}; } -XYZCoord Dubins::circleArc(const RRTPoint &starting_point, double beta, const XYZCoord ¢er, - double path_length) const { - // 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) - double angle = starting_point.psi + ((path_length / _radius) - HALF_PI) * sign(beta); - XYZCoord direction_vector{std::cos(angle), std::sin(angle), 0}; - return center + (direction_vector * _radius); +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) + double angle = starting_point.psi + ((path_length / _radius) - HALF_PI) * sign(beta); + XYZCoord direction_vector{std::cos(angle), std::sin(angle), 0}; + return center + (direction_vector * _radius); } -std::vector Dubins::generatePointsStraight(const RRTPoint &start, const RRTPoint &end, - const DubinsPath &path) const { - // 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; - - XYZCoord center_0 = findCenter(start, (path.beta_0 > 0) ? 'L' : 'R'); - XYZCoord center_2 = findCenter(end, (path.beta_2 > 0) ? 'L' : 'R'); - - XYZCoord initial_terminal_point{0, 0, 0}; // start of the straight secton - XYZCoord final_terminal_point{0, 0, 0}; // end of the straight section - - // finds starting and ending points of the _straight section_ - if (std::abs(path.beta_0) > 0) { - // Angle the path is going to turn along turn_1 - // (start direction) + (FROM CENTER OF CIRCLE, - // the angle traveled along curve - // * -1 if the plane is turning right) - double angle = start.psi + (std::abs(path.beta_0) - HALF_PI) * sign(path.beta_0); - initial_terminal_point = center_0 + _radius * XYZCoord{std::cos(angle), std::sin(angle), 0}; - } else { - initial_terminal_point = XYZCoord{start.coord.x, start.coord.y, start.coord.z}; - } - - if (std::abs(path.beta_2) > 0) { - // negative sign before beta_2 is because the path comes in through the back of the end - // vector - double angle = end.psi + (-std::abs(path.beta_2) - HALF_PI) * sign(path.beta_2); - final_terminal_point = center_2 + _radius * XYZCoord{std::cos(angle), std::sin(angle), 0}; - } else { - final_terminal_point = XYZCoord{end.coord.x, end.coord.y, 0}; - } - - double distance_straight = initial_terminal_point.distanceTo(final_terminal_point); - - // generates the points for the entire curve. - std::vector points_list; - 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 - // coefficient is the ratio of the straight distance that has been traversed. - // (current_distance_traved - (LENGTH_OF_FIRST_TURN_CURVED_PATH)) / - // length_of_the_straight_path - double coefficient = - (current_distance - (std::abs(path.beta_0) * _radius)) / distance_straight; - // convex linear combination to find the vector along the straight path between the - // initial and final point https://en.wikiversity.org/wiki/Convex_combination - points_list.emplace_back(coefficient * final_terminal_point + - (1 - coefficient) * initial_terminal_point); - } - } - points_list.emplace_back(XYZCoord{end.coord.x, end.coord.y, 0}); - - return points_list; +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; + + XYZCoord center_0 = findCenter(start, (path.beta_0 > 0) ? 'L' : 'R'); + XYZCoord center_2 = findCenter(end, (path.beta_2 > 0) ? 'L' : 'R'); + + XYZCoord initial_terminal_point{0, 0, 0}; // start of the straight secton + XYZCoord final_terminal_point{0, 0, 0}; // end of the straight section + + // finds starting and ending points of the _straight section_ + if (std::abs(path.beta_0) > 0) { + // Angle the path is going to turn along turn_1 + // (start direction) + (FROM CENTER OF CIRCLE, + // the angle traveled along curve + // * -1 if the plane is turning right) + double angle = start.psi + (std::abs(path.beta_0) - HALF_PI) * sign(path.beta_0); + initial_terminal_point = center_0 + _radius * XYZCoord{std::cos(angle), std::sin(angle), 0}; + } else { + initial_terminal_point = XYZCoord{start.coord.x, start.coord.y, start.coord.z}; + } + + if (std::abs(path.beta_2) > 0) { + // negative sign before beta_2 is because the path comes in through the back of the end + // vector + double angle = end.psi + (-std::abs(path.beta_2) - HALF_PI) * sign(path.beta_2); + final_terminal_point = center_2 + _radius * XYZCoord{std::cos(angle), std::sin(angle), 0}; + } else { + final_terminal_point = XYZCoord{end.coord.x, end.coord.y, 0}; + } + + double distance_straight = initial_terminal_point.distanceTo(final_terminal_point); + + // generates the points for the entire curve. + int n_points = std::max(1, (int)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 + points_list.emplace_back( + circleArc(end, path.beta_2, center_2, current_distance - total_distance)); + } else { // Straignt Section + // coefficient is the ratio of the straight distance that has been traversed. + // (current_distance_traved - (LENGTH_OF_FIRST_TURN_CURVED_PATH)) / + // length_of_the_straight_path + double coefficient = + (current_distance - (std::abs(path.beta_0) * _radius)) / distance_straight; + // convex linear combination to find the vector along the straight path between the + // initial and final point https://en.wikiversity.org/wiki/Convex_combination + points_list.emplace_back(coefficient * final_terminal_point + + (1 - coefficient) * initial_terminal_point); + } + } + points_list.emplace_back(XYZCoord{end.coord.x, end.coord.y, 0}); + + return points_list; } -std::vector Dubins::generatePointsCurve(const RRTPoint &start, const RRTPoint &end, - const DubinsPath &path) const { - // the arclength of all paths - double total_distance = - _radius * (std::abs(path.beta_2) + std::abs(path.beta_0) + std::abs(path.straight_dist)); - - XYZCoord center_0 = findCenter(start, (path.beta_0 > 0) ? 'L' : 'R'); - XYZCoord center_2 = findCenter(end, (path.beta_2 > 0) ? 'L' : 'R'); - - double half_intercenter_distance = center_0.distanceTo(center_2) / 2; - // uses pythagorean theorem to determine the center - XYZCoord center_1 = - 0.5 * (center_0 + center_2) // midpoint - + sign(path.beta_0) - // hypotnuse - 2r (distance between center0/2 and center 1) - // a - intercenter_distance / 2 (half of distance between center 0 and center 2) - * sqrt(std::pow(2 * _radius, 2) - std::pow(half_intercenter_distance, 2)) // - * findOrthogonalVector2D(center_2 - center_0) - .normalized(); // unit vector (direction vector) orthogonal to the displacement - // vector between the two centers - - // angle between x+ and the "terminal vector" between turn 1 and turn 2 - // i.e the angle from center_1 (x+) to center_0 - // angle of the displacement vector relative to x+ (horizontal) - 180 deg [supplimentary - // interior angle] - double psi_0 = std::atan2(center_1.y - center_0.y, center_1.x - center_0.x) - M_PI; - - std::vector points_list; - 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 - points_list.emplace_back( - circleArc(end, path.beta_2, center_2, current_distance - total_distance)); - } else { // Middle Turn - // angle relative to center 1 pointing around the curve - // starting angle - (1 if LRL, -1 if RLR, * (total angular distance - angular distance - // from first turn)) *note, it is subtracting because the sign of the middle curve is - // always opposite to the start curve - double angle = - psi_0 - (sign(path.beta_0) * (current_distance / _radius - std::abs(path.beta_0))); - XYZCoord direction_vector{std::cos(angle), std::sin(angle), 0}; - points_list.emplace_back(center_1 + _radius * direction_vector); - } - } - - points_list.emplace_back(XYZCoord{end.coord.x, end.coord.y, 0}); - - return points_list; +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)); + + XYZCoord center_0 = findCenter(start, (path.beta_0 > 0) ? 'L' : 'R'); + XYZCoord center_2 = findCenter(end, (path.beta_2 > 0) ? 'L' : 'R'); + + double half_intercenter_distance = center_0.distanceTo(center_2) / 2; + // uses pythagorean theorem to determine the center + XYZCoord center_1 = + 0.5 * (center_0 + center_2) // midpoint + + sign(path.beta_0) + // hypotnuse - 2r (distance between center0/2 and center 1) + // a - intercenter_distance / 2 (half of distance between center 0 and center 2) + * sqrt(std::pow(2 * _radius, 2) - std::pow(half_intercenter_distance, 2)) // + * findOrthogonalVector2D(center_2 - center_0) + .normalized(); // unit vector (direction vector) orthogonal to the displacement + // vector between the two centers + + // angle between x+ and the "terminal vector" between turn 1 and turn 2 + // i.e the angle from center_1 (x+) to center_0 + // angle of the displacement vector relative to x+ (horizontal) - 180 deg [supplimentary + // 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, (int)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 + points_list.emplace_back( + circleArc(end, path.beta_2, center_2, current_distance - total_distance)); + } else { // Middle Turn + // angle relative to center 1 pointing around the curve + // starting angle - (1 if LRL, -1 if RLR, * (total angular distance - angular distance + // from first turn)) *note, it is subtracting because the sign of the middle curve is + // always opposite to the start curve + double angle = + psi_0 - (sign(path.beta_0) * (current_distance / _radius - std::abs(path.beta_0))); + XYZCoord direction_vector{std::cos(angle), std::sin(angle), 0}; + points_list.emplace_back(center_1 + _radius * direction_vector); + } + } + + points_list.emplace_back(XYZCoord{end.coord.x, end.coord.y, 0}); + + return points_list; } -std::vector Dubins::generatePoints(const RRTPoint &start, const RRTPoint &end, - const DubinsPath &path, bool has_straight) const { - if (has_straight) { - return generatePointsStraight(start, end, path); - } +std::vector Dubins::generatePoints(const RRTPoint& start, const RRTPoint& end, + const DubinsPath& path, bool has_straight) { + if (has_straight) { + return generatePointsStraight(start, end, path); + } - return generatePointsCurve(start, end, path); + return generatePointsCurve(start, end, path); } -RRTOption Dubins::lsl(const RRTPoint &start, const RRTPoint &end, const XYZCoord ¢er_0, - const XYZCoord ¢er_2) const { - double straight_distance = center_0.distanceTo(center_2); +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 - double alpha = std::atan2(center_2.y - center_0.y, center_2.x - center_0.x); + // angle relative to horizontal + double alpha = std::atan2(center_2.y - center_0.y, center_2.x - center_0.x); - // difference in angle on the interval [0, 2pi] (CCW) - double beta_0 = mod(alpha - start.psi, TWO_PI); - double beta_2 = mod(end.psi - alpha, TWO_PI); + // difference in angle on the interval [0, 2pi] (CCW) + double beta_0 = mod(alpha - start.psi, TWO_PI); + double beta_2 = mod(end.psi - alpha, TWO_PI); - double total_distance = _radius * (beta_0 + beta_2) + straight_distance; + double total_distance = _radius * (beta_0 + beta_2) + straight_distance; - return RRTOption{total_distance, DubinsPath(beta_0, beta_2, straight_distance), true}; + 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 { - double straight_distance = center_0.distanceTo(center_2); +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 - double alpha = std::atan2(center_2.y - center_0.y, center_2.x - center_0.x); + // angle relative to horizontal + double alpha = std::atan2(center_2.y - center_0.y, center_2.x - center_0.x); - // offset betwen alpha and other vector [0,2pi] - // 1] calculates the CCW (positive) rotation from start vector to end vector - // (end_a - start_a) - // 2] takes the negative value of ^ - // 3] ^ % 2pi == the rotation CW in terms of positive radians - double beta_0 = mod(-(alpha - start.psi), TWO_PI); - double beta_2 = mod(-(end.psi - alpha), TWO_PI); + // offset betwen alpha and other vector [0,2pi] + // 1] calculates the CCW (positive) rotation from start vector to end vector + // (end_a - start_a) + // 2] takes the negative value of ^ + // 3] ^ % 2pi == the rotation CW in terms of positive radians + double beta_0 = mod(-(alpha - start.psi), TWO_PI); + double beta_2 = mod(-(end.psi - alpha), TWO_PI); - double total_distance = _radius * (beta_2 + beta_0) + straight_distance; + double total_distance = _radius * (beta_2 + beta_0) + straight_distance; - return RRTOption{total_distance, DubinsPath(-beta_0, -beta_2, straight_distance), true}; + 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 { - 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(); - - if (half_intercenter_distance < _radius) { - return RRTOption{std::numeric_limits::infinity(), DubinsPath(0, 0, 0), true}; - } - - // angle between intercenter displacement vector - // AND vector orthogonal to the "leave circle" vector - double alpha = std::acos(_radius / half_intercenter_distance); - // (INTERCENTER _reference_) - (ANGLE TO TERMINAL) - - // (STARTING VECTOR NORMAL TO CIRCLE) - // i.e. (angle to terminal relative to horizontal) - (start angle) - double beta_0 = mod(psi_0 - alpha - (start.psi - HALF_PI), TWO_PI); - // Same as ^, but shifted PI becuase psi_0 has been shifted PI - // (to face the opposite center) - double beta_2 = mod((M_PI + psi_0) - alpha - (end.psi + HALF_PI), TWO_PI); - - // pythagorean theroem to calculate distance off of known - // right trangle using intercenter/radius - double straight_distance = - 2 * std::sqrt(std::pow(half_intercenter_distance, 2) - std::pow(_radius, 2)); - double total_distance = _radius * (beta_0 + beta_2) + straight_distance; - - return RRTOption{total_distance, DubinsPath(beta_0, -beta_2, straight_distance), true}; +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(); + + if (half_intercenter_distance < _radius) { + return RRTOption{std::numeric_limits::infinity(), DubinsPath(0, 0, 0), true}; + } + + // angle between intercenter displacement vector + // AND vector orthogonal to the "leave circle" vector + double alpha = std::acos(_radius / half_intercenter_distance); + // (INTERCENTER _reference_) - (ANGLE TO TERMINAL) - + // (STARTING VECTOR NORMAL TO CIRCLE) + // i.e. (angle to terminal relative to horizontal) - (start angle) + double beta_0 = mod(psi_0 - alpha - (start.psi - HALF_PI), TWO_PI); + // Same as ^, but shifted PI becuase psi_0 has been shifted PI + // (to face the opposite center) + double beta_2 = mod((M_PI + psi_0) - alpha - (end.psi + HALF_PI), TWO_PI); + + // pythagorean theroem to calculate distance off of known + // right trangle using intercenter/radius + double straight_distance = + 2 * std::sqrt(std::pow(half_intercenter_distance, 2) - std::pow(_radius, 2)); + double total_distance = _radius * (beta_0 + beta_2) + straight_distance; + + 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 { - 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(); - - if (half_intercenter_distance < _radius) { - return RRTOption{std::numeric_limits::infinity(), DubinsPath(0, 0, 0), true}; - } - - // angle between intercenter displacement vector AND - // vector orthogonal to the "leave circle" vector - double alpha = std::acos(_radius / half_intercenter_distance); - // (STARTING_ANGLE) - (ANGLE TO TERMINAL) - // [assuming everything is relative to horizontal] - // i.e. (start ==> normal to circle) - - // (angle to connect center + - // angle between center an "leave circle" point) - double beta_0 = mod((start.psi + HALF_PI) - (psi_0 + alpha), TWO_PI); - // Same as ^, but shifted PI becuase psi_0 has been shifted PI - // (to face the opposite center) - double beta_2 = mod((end.psi - HALF_PI) - (alpha + psi_0 + M_PI), TWO_PI); - - // pythagorean theroem to calculate distance off of known - // right trangle using intercenter/radius - double straight_distance = - 2 * std::sqrt(std::pow(half_intercenter_distance, 2) - std::pow(_radius, 2)); - double total_distance = _radius * (beta_0 + beta_2) + straight_distance; - - return RRTOption{total_distance, DubinsPath(-beta_0, beta_2, straight_distance), true}; +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(); + + if (half_intercenter_distance < _radius) { + return RRTOption{std::numeric_limits::infinity(), DubinsPath(0, 0, 0), true}; + } + + // angle between intercenter displacement vector AND + // vector orthogonal to the "leave circle" vector + double alpha = std::acos(_radius / half_intercenter_distance); + // (STARTING_ANGLE) - (ANGLE TO TERMINAL) + // [assuming everything is relative to horizontal] + // i.e. (start ==> normal to circle) - + // (angle to connect center + + // angle between center an "leave circle" point) + double beta_0 = mod((start.psi + HALF_PI) - (psi_0 + alpha), TWO_PI); + // Same as ^, but shifted PI becuase psi_0 has been shifted PI + // (to face the opposite center) + double beta_2 = mod((end.psi - HALF_PI) - (alpha + psi_0 + M_PI), TWO_PI); + + // pythagorean theroem to calculate distance off of known + // right trangle using intercenter/radius + double straight_distance = + 2 * std::sqrt(std::pow(half_intercenter_distance, 2) - std::pow(_radius, 2)); + double total_distance = _radius * (beta_0 + beta_2) + straight_distance; + + 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 { - 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); - - if (intercenter_distance < 2 * _radius && intercenter_distance > 4 * _radius) { - return RRTOption{std::numeric_limits::infinity(), DubinsPath(0, 0, 0), false}; - } - - // angle formed by the connection of the radii (the isosoles triange) - // (using similar triangles) - // 2 * (sin of the triangle made by splitting the isocoles triangle in half - // (a similar triangle twice as large)) - // [half of gamma (splitting the right triangle in two)] - double gamma = 2 * std::asin(intercenter_distance / (4 * _radius)); - // bottom two angles of isocoles (180 - gamma) = 2 * theta - double theta = (M_PI - gamma) / 2; - // (ANGLE REQUIRED TO GET FROM START TO psi_0) + (THETA) - double beta_0 = mod(psi_0 - (start.psi - HALF_PI) + theta, TWO_PI); - // (ANGLE FROM psi_0 --> END) + (THETA) - // [psi_0 now needs to point towards center_0] - double beta_2 = mod((end.psi - HALF_PI) - (psi_0 + M_PI) + theta, TWO_PI); - - // beta_1 ==> (2PI - gamma) is positive angle that the plane flys through - double beta_1 = TWO_PI - gamma; - double total_distance = (beta_1 + beta_0 + beta_2) * _radius; - return RRTOption{total_distance, DubinsPath(beta_0, beta_2, -beta_1), false}; +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); + + if (intercenter_distance < 2 * _radius && intercenter_distance > 4 * _radius) { + return RRTOption{std::numeric_limits::infinity(), DubinsPath(0, 0, 0), false}; + } + + // angle formed by the connection of the radii (the isosoles triange) + // (using similar triangles) + // 2 * (sin of the triangle made by splitting the isocoles triangle in half + // (a similar triangle twice as large)) + // [half of gamma (splitting the right triangle in two)] + double gamma = 2 * std::asin(intercenter_distance / (4 * _radius)); + // bottom two angles of isocoles (180 - gamma) = 2 * theta + double theta = (M_PI - gamma) / 2; + // (ANGLE REQUIRED TO GET FROM START TO psi_0) + (THETA) + double beta_0 = mod(psi_0 - (start.psi - HALF_PI) + theta, TWO_PI); + // (ANGLE FROM psi_0 --> END) + (THETA) + // [psi_0 now needs to point towards center_0] + double beta_2 = mod((end.psi - HALF_PI) - (psi_0 + M_PI) + theta, TWO_PI); + + // beta_1 ==> (2PI - gamma) is positive angle that the plane flys through + double beta_1 = TWO_PI - gamma; + double total_distance = (beta_1 + beta_0 + beta_2) * _radius; + 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 { - 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); - - if (intercenter_distance < 2 * _radius && intercenter_distance > 4 * _radius) { - return RRTOption{std::numeric_limits::infinity(), DubinsPath(0, 0, 0), false}; - } - - // angle formed by the connection of the radii (the isosoles triange) - // (using similar triangles) - // 2 * (sin of the triangle made by splitting the isocoles triangle in half - // (a similar triangle twice as large)) - // [half of gamma (splitting the right triangle in two)] - double gamma = 2 * std::asin(intercenter_distance / (4 * _radius)); - double theta = (M_PI - gamma) / 2; - - // same as lrl, except its a negative angle because its a right turn - // (ANGLE REQUIRED TO GET FROM START TO psi_0) + (THETA) - double beta_0 = mod((start.psi + HALF_PI) - psi_0 + theta, TWO_PI); - // (ANGLE FROM psi_0 --> END) + (THETA) - // [psi_0 now needs to point towards center_0] - double beta_2 = mod((psi_0 + HALF_PI) - end.psi + theta, TWO_PI); - - // beta_1 ==> (2PI - gamma) is positive angle that the plane flys through - double beta_1 = TWO_PI - gamma; - double total_distance = (beta_1 + beta_0 + beta_2) * _radius; - return RRTOption{total_distance, DubinsPath(-beta_0, -beta_2, beta_1), false}; +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); + + if (intercenter_distance < 2 * _radius && intercenter_distance > 4 * _radius) { + return RRTOption{std::numeric_limits::infinity(), DubinsPath(0, 0, 0), false}; + } + + // angle formed by the connection of the radii (the isosoles triange) + // (using similar triangles) + // 2 * (sin of the triangle made by splitting the isocoles triangle in half + // (a similar triangle twice as large)) + // [half of gamma (splitting the right triangle in two)] + double gamma = 2 * std::asin(intercenter_distance / (4 * _radius)); + double theta = (M_PI - gamma) / 2; + + // same as lrl, except its a negative angle because its a right turn + // (ANGLE REQUIRED TO GET FROM START TO psi_0) + (THETA) + double beta_0 = mod((start.psi + HALF_PI) - psi_0 + theta, TWO_PI); + // (ANGLE FROM psi_0 --> END) + (THETA) + // [psi_0 now needs to point towards center_0] + double beta_2 = mod((psi_0 + HALF_PI) - end.psi + theta, TWO_PI); + + // beta_1 ==> (2PI - gamma) is positive angle that the plane flys through + double beta_1 = TWO_PI - gamma; + double total_distance = (beta_1 + beta_0 + beta_2) * _radius; + 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 { - XYZCoord center_0_left = findCenter(start, 'L'); - XYZCoord center_0_right = findCenter(start, 'R'); - XYZCoord center_2_left = findCenter(end, 'L'); - XYZCoord center_2_right = findCenter(end, 'R'); - - std::vector options = { - lsl(start, end, center_0_left, center_2_left), - rsr(start, end, center_0_right, center_2_right), - rsl(start, end, center_0_right, center_2_left), - lsr(start, end, center_0_left, center_2_right) - //, - // lrl(start, end, center_0_left, center_2_left), - // rlr(start, end, center_0_right, center_2_right) - }; - - if (sort) { - std::sort(options.begin(), options.end(), compareRRTOptionLength); - } - - return options; +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'); + XYZCoord center_2_right = findCenter(end, 'R'); + + std::vector options = { + lsl(start, end, center_0_left, center_2_left), + rsr(start, end, center_0_right, center_2_right), + rsl(start, end, center_0_right, center_2_left), + lsr(start, end, center_0_left, center_2_right) + //, + // lrl(start, end, center_0_left, center_2_left), + // rlr(start, end, center_0_right, center_2_right) + }; + + return options; } -std::vector Dubins::dubinsPath(const RRTPoint &start, const RRTPoint &end) const { - 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); +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 { - std::vector options = allOptions(start, end); - return *std::min_element(options.begin(), options.end(), compareRRTOptionLength); +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/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..d9ba1c32 100644 --- a/src/pathing/tree.cpp +++ b/src/pathing/tree.cpp @@ -1,5 +1,6 @@ #include "pathing/tree.hpp" +#include #include #include #include @@ -71,8 +72,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 +91,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 +218,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 +307,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 +436,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 +458,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/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)); From 794744627324ccc8d5b10a47e5875748e4eff7d0 Mon Sep 17 00:00:00 2001 From: Christopher Lee Date: Fri, 31 Jul 2026 11:22:14 -0700 Subject: [PATCH 2/2] chore: lint --- CMakeLists.txt | 2 +- include/network/serialize.hpp | 24 +- include/network/udp_client.hpp | 2 +- include/pathing/dubins.hpp | 432 +++++++++++----------- include/utilities/http.hpp | 4 +- src/camera/interface.cpp | 3 + src/camera/mock.cpp | 7 +- src/camera/rpi.cpp | 6 +- src/core/mission_parameters.cpp | 2 + src/core/mission_state.cpp | 2 + src/core/obc.cpp | 5 +- src/cv/aggregator.cpp | 5 + src/cv/clustering.cpp | 3 +- src/cv/hdr.cpp | 3 + src/cv/localization.cpp | 630 ++++++++++++++++---------------- src/cv/mapping.cpp | 1 + src/cv/pipeline.cpp | 3 + src/cv/utilities.cpp | 3 + src/cv/yolo.cpp | 3 + src/network/airdrop_client.cpp | 1 + src/network/gcs_routes.cpp | 4 + src/network/mavlink.cpp | 1 + src/network/udp_client.cpp | 2 + src/network/udp_server.cpp | 4 + src/pathing/dubins.cpp | 594 +++++++++++++++--------------- src/pathing/mission_path.cpp | 20 +- src/pathing/plotting.cpp | 10 +- src/pathing/tree.cpp | 2 + src/ticks/manual_landing.cpp | 1 + src/ticks/mission_prep.cpp | 1 + src/utilities/datatypes.cpp | 2 + 31 files changed, 906 insertions(+), 876 deletions(-) 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 24108c56..9023c542 100644 --- a/include/pathing/dubins.hpp +++ b/include/pathing/dubins.hpp @@ -86,244 +86,224 @@ XYZCoord findOrthogonalVector2D(const XYZCoord& vector); XYZCoord halfDisplacement(const XYZCoord& vector1, const XYZCoord& vector2); namespace Dubins { - inline double _radius; - inline double _point_separation; +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); +/** + * 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& 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); +/** + * 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); +/** + * 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); +/** + * 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& 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 (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& 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& 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& 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& 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& 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& 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& 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& 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& 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& 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 + * @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 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); - /** - * 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); +/** + * 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); -}; +}; // namespace Dubins #endif // INCLUDE_PATHING_DUBINS_HPP_ 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 e7c1dac0..1807d3e4 100644 --- a/src/pathing/dubins.cpp +++ b/src/pathing/dubins.cpp @@ -1,13 +1,13 @@ #include "pathing/dubins.hpp" +#include #include #include #include +#include #include "utilities/datatypes.hpp" -using namespace Dubins; - /** * Notes from Christopher: * @@ -33,369 +33,365 @@ using namespace Dubins; * if right turn ==> -beta) */ - enum turn { - FIRST_TURN, - MIDDLE_TURN, - LAST_TURN - }; +enum turn { FIRST_TURN, MIDDLE_TURN, LAST_TURN }; template int sign(T number) { - return (T(0) < number) - (number < T(0)); + return (T(0) < number) - (number < T(0)); } double mod(double dividend, double divisor) { - 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) { - return first.length < second.length; + return first.length < second.length; } XYZCoord findOrthogonalVector2D(const XYZCoord& vector) { - return XYZCoord{-vector.y, vector.x, vector.z}; + return XYZCoord{-vector.y, vector.x, vector.z}; } XYZCoord halfDisplacement(const XYZCoord& vector1, const XYZCoord& vector2) { - return 0.5 * (vector1 - vector2); + return 0.5 * (vector1 - vector2); } XYZCoord Dubins::findCenter(const RRTPoint& point, char side) { - assert(side == 'L' || side == 'R'); + assert(side == 'L' || side == 'R'); - // creates a right angle between the RRTPoint vector towards the center - // left is 90 deg CCW, right is 90 deg CW - double angle = point.psi + (side == 'L' ? HALF_PI : -HALF_PI); + // creates a right angle between the RRTPoint vector towards the center + // left is 90 deg CCW, right is 90 deg CW + double angle = point.psi + (side == 'L' ? HALF_PI : -HALF_PI); - // 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}; + // 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}; } 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) - double angle = starting_point.psi + ((path_length / _radius) - HALF_PI) * sign(beta); - XYZCoord direction_vector{std::cos(angle), std::sin(angle), 0}; - return center + (direction_vector * _radius); + 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) + double angle = starting_point.psi + ((path_length / _radius) - HALF_PI) * sign(beta); + XYZCoord direction_vector{std::cos(angle), std::sin(angle), 0}; + return center + (direction_vector * _radius); } 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; - - XYZCoord center_0 = findCenter(start, (path.beta_0 > 0) ? 'L' : 'R'); - XYZCoord center_2 = findCenter(end, (path.beta_2 > 0) ? 'L' : 'R'); - - XYZCoord initial_terminal_point{0, 0, 0}; // start of the straight secton - XYZCoord final_terminal_point{0, 0, 0}; // end of the straight section - - // finds starting and ending points of the _straight section_ - if (std::abs(path.beta_0) > 0) { - // Angle the path is going to turn along turn_1 - // (start direction) + (FROM CENTER OF CIRCLE, - // the angle traveled along curve - // * -1 if the plane is turning right) - double angle = start.psi + (std::abs(path.beta_0) - HALF_PI) * sign(path.beta_0); - initial_terminal_point = center_0 + _radius * XYZCoord{std::cos(angle), std::sin(angle), 0}; - } else { - initial_terminal_point = XYZCoord{start.coord.x, start.coord.y, start.coord.z}; - } - - if (std::abs(path.beta_2) > 0) { - // negative sign before beta_2 is because the path comes in through the back of the end - // vector - double angle = end.psi + (-std::abs(path.beta_2) - HALF_PI) * sign(path.beta_2); - final_terminal_point = center_2 + _radius * XYZCoord{std::cos(angle), std::sin(angle), 0}; - } else { - final_terminal_point = XYZCoord{end.coord.x, end.coord.y, 0}; - } - - double distance_straight = initial_terminal_point.distanceTo(final_terminal_point); - - // generates the points for the entire curve. - int n_points = std::max(1, (int)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 - points_list.emplace_back( - circleArc(end, path.beta_2, center_2, current_distance - total_distance)); - } else { // Straignt Section - // coefficient is the ratio of the straight distance that has been traversed. - // (current_distance_traved - (LENGTH_OF_FIRST_TURN_CURVED_PATH)) / - // length_of_the_straight_path - double coefficient = - (current_distance - (std::abs(path.beta_0) * _radius)) / distance_straight; - // convex linear combination to find the vector along the straight path between the - // initial and final point https://en.wikiversity.org/wiki/Convex_combination - points_list.emplace_back(coefficient * final_terminal_point + - (1 - coefficient) * initial_terminal_point); - } - } - points_list.emplace_back(XYZCoord{end.coord.x, end.coord.y, 0}); - - return points_list; + 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; + + XYZCoord center_0 = findCenter(start, (path.beta_0 > 0) ? 'L' : 'R'); + XYZCoord center_2 = findCenter(end, (path.beta_2 > 0) ? 'L' : 'R'); + + XYZCoord initial_terminal_point{0, 0, 0}; // start of the straight secton + XYZCoord final_terminal_point{0, 0, 0}; // end of the straight section + + // finds starting and ending points of the _straight section_ + if (std::abs(path.beta_0) > 0) { + // Angle the path is going to turn along turn_1 + // (start direction) + (FROM CENTER OF CIRCLE, + // the angle traveled along curve + // * -1 if the plane is turning right) + double angle = start.psi + (std::abs(path.beta_0) - HALF_PI) * sign(path.beta_0); + initial_terminal_point = center_0 + _radius * XYZCoord{std::cos(angle), std::sin(angle), 0}; + } else { + initial_terminal_point = XYZCoord{start.coord.x, start.coord.y, start.coord.z}; + } + + if (std::abs(path.beta_2) > 0) { + // negative sign before beta_2 is because the path comes in through the back of the end + // vector + double angle = end.psi + (-std::abs(path.beta_2) - HALF_PI) * sign(path.beta_2); + final_terminal_point = center_2 + _radius * XYZCoord{std::cos(angle), std::sin(angle), 0}; + } else { + final_terminal_point = XYZCoord{end.coord.x, end.coord.y, 0}; + } + + 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 + points_list.emplace_back( + circleArc(end, path.beta_2, center_2, current_distance - total_distance)); + } else { // Straignt Section + // coefficient is the ratio of the straight distance that has been traversed. + // (current_distance_traved - (LENGTH_OF_FIRST_TURN_CURVED_PATH)) / + // length_of_the_straight_path + double coefficient = + (current_distance - (std::abs(path.beta_0) * _radius)) / distance_straight; + // convex linear combination to find the vector along the straight path between the + // initial and final point https://en.wikiversity.org/wiki/Convex_combination + points_list.emplace_back(coefficient * final_terminal_point + + (1 - coefficient) * initial_terminal_point); + } + } + points_list.emplace_back(XYZCoord{end.coord.x, end.coord.y, 0}); + + return points_list; } 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)); - - XYZCoord center_0 = findCenter(start, (path.beta_0 > 0) ? 'L' : 'R'); - XYZCoord center_2 = findCenter(end, (path.beta_2 > 0) ? 'L' : 'R'); - - double half_intercenter_distance = center_0.distanceTo(center_2) / 2; - // uses pythagorean theorem to determine the center - XYZCoord center_1 = - 0.5 * (center_0 + center_2) // midpoint - + sign(path.beta_0) - // hypotnuse - 2r (distance between center0/2 and center 1) - // a - intercenter_distance / 2 (half of distance between center 0 and center 2) - * sqrt(std::pow(2 * _radius, 2) - std::pow(half_intercenter_distance, 2)) // - * findOrthogonalVector2D(center_2 - center_0) - .normalized(); // unit vector (direction vector) orthogonal to the displacement - // vector between the two centers - - // angle between x+ and the "terminal vector" between turn 1 and turn 2 - // i.e the angle from center_1 (x+) to center_0 - // angle of the displacement vector relative to x+ (horizontal) - 180 deg [supplimentary - // 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, (int)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 - points_list.emplace_back( - circleArc(end, path.beta_2, center_2, current_distance - total_distance)); - } else { // Middle Turn - // angle relative to center 1 pointing around the curve - // starting angle - (1 if LRL, -1 if RLR, * (total angular distance - angular distance - // from first turn)) *note, it is subtracting because the sign of the middle curve is - // always opposite to the start curve - double angle = - psi_0 - (sign(path.beta_0) * (current_distance / _radius - std::abs(path.beta_0))); - XYZCoord direction_vector{std::cos(angle), std::sin(angle), 0}; - points_list.emplace_back(center_1 + _radius * direction_vector); - } - } - - points_list.emplace_back(XYZCoord{end.coord.x, end.coord.y, 0}); - - return points_list; + 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)); + + XYZCoord center_0 = findCenter(start, (path.beta_0 > 0) ? 'L' : 'R'); + XYZCoord center_2 = findCenter(end, (path.beta_2 > 0) ? 'L' : 'R'); + + double half_intercenter_distance = center_0.distanceTo(center_2) / 2; + // uses pythagorean theorem to determine the center + XYZCoord center_1 = + 0.5 * (center_0 + center_2) // midpoint + + sign(path.beta_0) + // hypotnuse - 2r (distance between center0/2 and center 1) + // a - intercenter_distance / 2 (half of distance between center 0 and center 2) + * sqrt(std::pow(2 * _radius, 2) - std::pow(half_intercenter_distance, 2)) // + * findOrthogonalVector2D(center_2 - center_0) + .normalized(); // unit vector (direction vector) orthogonal to the displacement + // vector between the two centers + + // angle between x+ and the "terminal vector" between turn 1 and turn 2 + // i.e the angle from center_1 (x+) to center_0 + // angle of the displacement vector relative to x+ (horizontal) - 180 deg [supplimentary + // 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 + 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 + points_list.emplace_back( + circleArc(end, path.beta_2, center_2, current_distance - total_distance)); + } else { // Middle Turn + // angle relative to center 1 pointing around the curve + // starting angle - (1 if LRL, -1 if RLR, * (total angular distance - angular distance + // from first turn)) *note, it is subtracting because the sign of the middle curve is + // always opposite to the start curve + double angle = + psi_0 - (sign(path.beta_0) * (current_distance / _radius - std::abs(path.beta_0))); + XYZCoord direction_vector{std::cos(angle), std::sin(angle), 0}; + points_list.emplace_back(center_1 + _radius * direction_vector); + } + } + + points_list.emplace_back(XYZCoord{end.coord.x, end.coord.y, 0}); + + return points_list; } std::vector Dubins::generatePoints(const RRTPoint& start, const RRTPoint& end, - const DubinsPath& path, bool has_straight) { - if (has_straight) { - return generatePointsStraight(start, end, path); - } + const DubinsPath& path, bool has_straight) { + if (has_straight) { + return generatePointsStraight(start, end, path); + } - return generatePointsCurve(start, end, path); + return generatePointsCurve(start, end, path); } 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); + const XYZCoord& center_2) { + double straight_distance = center_0.distanceTo(center_2); - // angle relative to horizontal - double alpha = std::atan2(center_2.y - center_0.y, center_2.x - center_0.x); + // angle relative to horizontal + double alpha = std::atan2(center_2.y - center_0.y, center_2.x - center_0.x); - // difference in angle on the interval [0, 2pi] (CCW) - double beta_0 = mod(alpha - start.psi, TWO_PI); - double beta_2 = mod(end.psi - alpha, TWO_PI); + // difference in angle on the interval [0, 2pi] (CCW) + double beta_0 = mod(alpha - start.psi, TWO_PI); + double beta_2 = mod(end.psi - alpha, TWO_PI); - double total_distance = _radius * (beta_0 + beta_2) + straight_distance; + double total_distance = _radius * (beta_0 + beta_2) + straight_distance; - return RRTOption{total_distance, DubinsPath(beta_0, beta_2, straight_distance), true}; + return RRTOption{total_distance, DubinsPath(beta_0, beta_2, straight_distance), true}; } 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); + const XYZCoord& center_2) { + double straight_distance = center_0.distanceTo(center_2); - // angle relative to horizontal - double alpha = std::atan2(center_2.y - center_0.y, center_2.x - center_0.x); + // angle relative to horizontal + double alpha = std::atan2(center_2.y - center_0.y, center_2.x - center_0.x); - // offset betwen alpha and other vector [0,2pi] - // 1] calculates the CCW (positive) rotation from start vector to end vector - // (end_a - start_a) - // 2] takes the negative value of ^ - // 3] ^ % 2pi == the rotation CW in terms of positive radians - double beta_0 = mod(-(alpha - start.psi), TWO_PI); - double beta_2 = mod(-(end.psi - alpha), TWO_PI); + // offset betwen alpha and other vector [0,2pi] + // 1] calculates the CCW (positive) rotation from start vector to end vector + // (end_a - start_a) + // 2] takes the negative value of ^ + // 3] ^ % 2pi == the rotation CW in terms of positive radians + double beta_0 = mod(-(alpha - start.psi), TWO_PI); + double beta_2 = mod(-(end.psi - alpha), TWO_PI); - double total_distance = _radius * (beta_2 + beta_0) + straight_distance; + double total_distance = _radius * (beta_2 + beta_0) + straight_distance; - return RRTOption{total_distance, DubinsPath(-beta_0, -beta_2, straight_distance), true}; + return RRTOption{total_distance, DubinsPath(-beta_0, -beta_2, straight_distance), true}; } 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(); - - if (half_intercenter_distance < _radius) { - return RRTOption{std::numeric_limits::infinity(), DubinsPath(0, 0, 0), true}; - } - - // angle between intercenter displacement vector - // AND vector orthogonal to the "leave circle" vector - double alpha = std::acos(_radius / half_intercenter_distance); - // (INTERCENTER _reference_) - (ANGLE TO TERMINAL) - - // (STARTING VECTOR NORMAL TO CIRCLE) - // i.e. (angle to terminal relative to horizontal) - (start angle) - double beta_0 = mod(psi_0 - alpha - (start.psi - HALF_PI), TWO_PI); - // Same as ^, but shifted PI becuase psi_0 has been shifted PI - // (to face the opposite center) - double beta_2 = mod((M_PI + psi_0) - alpha - (end.psi + HALF_PI), TWO_PI); - - // pythagorean theroem to calculate distance off of known - // right trangle using intercenter/radius - double straight_distance = - 2 * std::sqrt(std::pow(half_intercenter_distance, 2) - std::pow(_radius, 2)); - double total_distance = _radius * (beta_0 + beta_2) + straight_distance; - - return RRTOption{total_distance, DubinsPath(beta_0, -beta_2, straight_distance), true}; + 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(); + + if (half_intercenter_distance < _radius) { + return RRTOption{std::numeric_limits::infinity(), DubinsPath(0, 0, 0), true}; + } + + // angle between intercenter displacement vector + // AND vector orthogonal to the "leave circle" vector + double alpha = std::acos(_radius / half_intercenter_distance); + // (INTERCENTER _reference_) - (ANGLE TO TERMINAL) - + // (STARTING VECTOR NORMAL TO CIRCLE) + // i.e. (angle to terminal relative to horizontal) - (start angle) + double beta_0 = mod(psi_0 - alpha - (start.psi - HALF_PI), TWO_PI); + // Same as ^, but shifted PI becuase psi_0 has been shifted PI + // (to face the opposite center) + double beta_2 = mod((M_PI + psi_0) - alpha - (end.psi + HALF_PI), TWO_PI); + + // pythagorean theroem to calculate distance off of known + // right trangle using intercenter/radius + double straight_distance = + 2 * std::sqrt(std::pow(half_intercenter_distance, 2) - std::pow(_radius, 2)); + double total_distance = _radius * (beta_0 + beta_2) + straight_distance; + + return RRTOption{total_distance, DubinsPath(beta_0, -beta_2, straight_distance), true}; } 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(); - - if (half_intercenter_distance < _radius) { - return RRTOption{std::numeric_limits::infinity(), DubinsPath(0, 0, 0), true}; - } - - // angle between intercenter displacement vector AND - // vector orthogonal to the "leave circle" vector - double alpha = std::acos(_radius / half_intercenter_distance); - // (STARTING_ANGLE) - (ANGLE TO TERMINAL) - // [assuming everything is relative to horizontal] - // i.e. (start ==> normal to circle) - - // (angle to connect center + - // angle between center an "leave circle" point) - double beta_0 = mod((start.psi + HALF_PI) - (psi_0 + alpha), TWO_PI); - // Same as ^, but shifted PI becuase psi_0 has been shifted PI - // (to face the opposite center) - double beta_2 = mod((end.psi - HALF_PI) - (alpha + psi_0 + M_PI), TWO_PI); - - // pythagorean theroem to calculate distance off of known - // right trangle using intercenter/radius - double straight_distance = - 2 * std::sqrt(std::pow(half_intercenter_distance, 2) - std::pow(_radius, 2)); - double total_distance = _radius * (beta_0 + beta_2) + straight_distance; - - return RRTOption{total_distance, DubinsPath(-beta_0, beta_2, straight_distance), true}; + 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(); + + if (half_intercenter_distance < _radius) { + return RRTOption{std::numeric_limits::infinity(), DubinsPath(0, 0, 0), true}; + } + + // angle between intercenter displacement vector AND + // vector orthogonal to the "leave circle" vector + double alpha = std::acos(_radius / half_intercenter_distance); + // (STARTING_ANGLE) - (ANGLE TO TERMINAL) + // [assuming everything is relative to horizontal] + // i.e. (start ==> normal to circle) - + // (angle to connect center + + // angle between center an "leave circle" point) + double beta_0 = mod((start.psi + HALF_PI) - (psi_0 + alpha), TWO_PI); + // Same as ^, but shifted PI becuase psi_0 has been shifted PI + // (to face the opposite center) + double beta_2 = mod((end.psi - HALF_PI) - (alpha + psi_0 + M_PI), TWO_PI); + + // pythagorean theroem to calculate distance off of known + // right trangle using intercenter/radius + double straight_distance = + 2 * std::sqrt(std::pow(half_intercenter_distance, 2) - std::pow(_radius, 2)); + double total_distance = _radius * (beta_0 + beta_2) + straight_distance; + + return RRTOption{total_distance, DubinsPath(-beta_0, beta_2, straight_distance), true}; } 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); - - if (intercenter_distance < 2 * _radius && intercenter_distance > 4 * _radius) { - return RRTOption{std::numeric_limits::infinity(), DubinsPath(0, 0, 0), false}; - } - - // angle formed by the connection of the radii (the isosoles triange) - // (using similar triangles) - // 2 * (sin of the triangle made by splitting the isocoles triangle in half - // (a similar triangle twice as large)) - // [half of gamma (splitting the right triangle in two)] - double gamma = 2 * std::asin(intercenter_distance / (4 * _radius)); - // bottom two angles of isocoles (180 - gamma) = 2 * theta - double theta = (M_PI - gamma) / 2; - // (ANGLE REQUIRED TO GET FROM START TO psi_0) + (THETA) - double beta_0 = mod(psi_0 - (start.psi - HALF_PI) + theta, TWO_PI); - // (ANGLE FROM psi_0 --> END) + (THETA) - // [psi_0 now needs to point towards center_0] - double beta_2 = mod((end.psi - HALF_PI) - (psi_0 + M_PI) + theta, TWO_PI); - - // beta_1 ==> (2PI - gamma) is positive angle that the plane flys through - double beta_1 = TWO_PI - gamma; - double total_distance = (beta_1 + beta_0 + beta_2) * _radius; - return RRTOption{total_distance, DubinsPath(beta_0, beta_2, -beta_1), false}; + 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); + + if (intercenter_distance < 2 * _radius && intercenter_distance > 4 * _radius) { + return RRTOption{std::numeric_limits::infinity(), DubinsPath(0, 0, 0), false}; + } + + // angle formed by the connection of the radii (the isosoles triange) + // (using similar triangles) + // 2 * (sin of the triangle made by splitting the isocoles triangle in half + // (a similar triangle twice as large)) + // [half of gamma (splitting the right triangle in two)] + double gamma = 2 * std::asin(intercenter_distance / (4 * _radius)); + // bottom two angles of isocoles (180 - gamma) = 2 * theta + double theta = (M_PI - gamma) / 2; + // (ANGLE REQUIRED TO GET FROM START TO psi_0) + (THETA) + double beta_0 = mod(psi_0 - (start.psi - HALF_PI) + theta, TWO_PI); + // (ANGLE FROM psi_0 --> END) + (THETA) + // [psi_0 now needs to point towards center_0] + double beta_2 = mod((end.psi - HALF_PI) - (psi_0 + M_PI) + theta, TWO_PI); + + // beta_1 ==> (2PI - gamma) is positive angle that the plane flys through + double beta_1 = TWO_PI - gamma; + double total_distance = (beta_1 + beta_0 + beta_2) * _radius; + return RRTOption{total_distance, DubinsPath(beta_0, beta_2, -beta_1), false}; } 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); - - if (intercenter_distance < 2 * _radius && intercenter_distance > 4 * _radius) { - return RRTOption{std::numeric_limits::infinity(), DubinsPath(0, 0, 0), false}; - } - - // angle formed by the connection of the radii (the isosoles triange) - // (using similar triangles) - // 2 * (sin of the triangle made by splitting the isocoles triangle in half - // (a similar triangle twice as large)) - // [half of gamma (splitting the right triangle in two)] - double gamma = 2 * std::asin(intercenter_distance / (4 * _radius)); - double theta = (M_PI - gamma) / 2; - - // same as lrl, except its a negative angle because its a right turn - // (ANGLE REQUIRED TO GET FROM START TO psi_0) + (THETA) - double beta_0 = mod((start.psi + HALF_PI) - psi_0 + theta, TWO_PI); - // (ANGLE FROM psi_0 --> END) + (THETA) - // [psi_0 now needs to point towards center_0] - double beta_2 = mod((psi_0 + HALF_PI) - end.psi + theta, TWO_PI); - - // beta_1 ==> (2PI - gamma) is positive angle that the plane flys through - double beta_1 = TWO_PI - gamma; - double total_distance = (beta_1 + beta_0 + beta_2) * _radius; - return RRTOption{total_distance, DubinsPath(-beta_0, -beta_2, beta_1), false}; + 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); + + if (intercenter_distance < 2 * _radius && intercenter_distance > 4 * _radius) { + return RRTOption{std::numeric_limits::infinity(), DubinsPath(0, 0, 0), false}; + } + + // angle formed by the connection of the radii (the isosoles triange) + // (using similar triangles) + // 2 * (sin of the triangle made by splitting the isocoles triangle in half + // (a similar triangle twice as large)) + // [half of gamma (splitting the right triangle in two)] + double gamma = 2 * std::asin(intercenter_distance / (4 * _radius)); + double theta = (M_PI - gamma) / 2; + + // same as lrl, except its a negative angle because its a right turn + // (ANGLE REQUIRED TO GET FROM START TO psi_0) + (THETA) + double beta_0 = mod((start.psi + HALF_PI) - psi_0 + theta, TWO_PI); + // (ANGLE FROM psi_0 --> END) + (THETA) + // [psi_0 now needs to point towards center_0] + double beta_2 = mod((psi_0 + HALF_PI) - end.psi + theta, TWO_PI); + + // beta_1 ==> (2PI - gamma) is positive angle that the plane flys through + double beta_1 = TWO_PI - gamma; + double total_distance = (beta_1 + beta_0 + beta_2) * _radius; + return RRTOption{total_distance, DubinsPath(-beta_0, -beta_2, beta_1), false}; } 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'); - XYZCoord center_2_right = findCenter(end, 'R'); - - std::vector options = { - lsl(start, end, center_0_left, center_2_left), - rsr(start, end, center_0_right, center_2_right), - rsl(start, end, center_0_right, center_2_left), - lsr(start, end, center_0_left, center_2_right) - //, - // lrl(start, end, center_0_left, center_2_left), - // rlr(start, end, center_0_right, center_2_right) - }; - - return options; + XYZCoord center_0_left = findCenter(start, 'L'); + XYZCoord center_0_right = findCenter(start, 'R'); + XYZCoord center_2_left = findCenter(end, 'L'); + XYZCoord center_2_right = findCenter(end, 'R'); + + std::vector options = { + lsl(start, end, center_0_left, center_2_left), + rsr(start, end, center_0_right, center_2_right), + rsl(start, end, center_0_right, center_2_left), + lsr(start, end, center_0_left, center_2_right) + //, + // lrl(start, end, center_0_left, center_2_left), + // rlr(start, end, center_0_right, center_2_right) + }; + + return options; } 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); + 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) { - std::vector options = allOptions(start, end); - return *std::min_element(options.begin(), options.end(), compareRRTOptionLength); + 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/tree.cpp b/src/pathing/tree.cpp index d9ba1c32..25695628 100644 --- a/src/pathing/tree.cpp +++ b/src/pathing/tree.cpp @@ -1,6 +1,8 @@ #include "pathing/tree.hpp" #include +#include +#include #include #include #include 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"