diff --git a/include/pathing/dubins.hpp b/include/pathing/dubins.hpp index 9023c542..4a773ce9 100644 --- a/include/pathing/dubins.hpp +++ b/include/pathing/dubins.hpp @@ -9,6 +9,8 @@ #include "utilities/datatypes.hpp" struct DubinsPath { + // members left indeterminate; only needed so this can live in a std::array + DubinsPath() = default; DubinsPath(double beta_0, double beta_2, double straight_dist) : beta_0(beta_0), beta_2(beta_2), straight_dist(straight_dist) {} @@ -21,6 +23,8 @@ struct DubinsPath { }; struct RRTOption { + // members left indeterminate; only needed so this can live in a std::array + RRTOption() = default; RRTOption(double length, DubinsPath dubins_path, bool has_straight) : length(length), dubins_path(dubins_path), has_straight(has_straight) {} @@ -29,6 +33,21 @@ struct RRTOption { bool has_straight; // if this option has a straight path or not }; +/** + * One leg of a flight: a dubins path, and the vector it lands on. + * + * Every option is generated to reach a known vector, so that vector is carried + * along with it instead of being recovered from the path parameters later on. + */ +struct PathSegment { + // members left indeterminate; only needed so this can live in a std::array + PathSegment() = default; + PathSegment(const RRTPoint& end, const RRTOption& option) : end(end), option(option) {} + + RRTPoint end; // the vector the leg ends on + RRTOption option; // the dubins path flown to get there +}; + /** * Reproduction of np.sign() function as used in the older obc * from @@ -149,6 +168,20 @@ std::vector generatePointsCurve(const RRTPoint& start, const RRTPoint& std::vector generatePoints(const RRTPoint& start, const RRTPoint& end, const DubinsPath& path, bool has_straight); +/** + * Generates the points along a sequence of dubins paths + * + * The segments are flown back to back, each one starting on the vector the + * previous one ended on. The first point of every segment is dropped, as it is + * either the last point of the previous segment or the point the plane is + * already at. + * + * @param start ==> vector the path is flown from + * @param segments ==> the dubins paths to fly, in order, with their end vectors + * @return ==> a list of points along the entire path + */ +std::vector generatePath(const RRTPoint& start, const std::vector& segments); + /** * First, the straight distance (it turns out) is equal to the * distance between the two centers diff --git a/include/pathing/environment.hpp b/include/pathing/environment.hpp index 0b2788fb..be7a0261 100644 --- a/include/pathing/environment.hpp +++ b/include/pathing/environment.hpp @@ -74,6 +74,31 @@ namespace Environment { */ bool isPathInBounds(const std::vector& path); + /** + * Check whether an entire Dubins path is in bounds, analytically. + * + * ONLY HANDLES CSC PATHS [LSL, RSR, LSR, RSL] + * + * @param[in] start ==> the start vector of the path + * @param[in] end ==> the end vector of the path + * @param[in] option ==> the Dubins option connecting start to end + * @return ==> true if every point of the path is in bounds, false otherwise + */ + bool isDubinsPathInBounds(const RRTPoint& start, const RRTPoint& end, const RRTOption& option); + + /** + * Check whether a circular arc is in bounds + * + * Checks for containment then edge intersections. + * + * @param[in] center ==> the center of the arc's circle + * @param[in] radius ==> the radius of the arc's circle + * @param[in] start_angle ==> angle from the center to the arc's first point (0 is +x, CCW positive) + * @param[in] sweep ==> signed angle swept, CCW positive (i.e. a DubinsPath beta) + * @return ==> whether or not the arc is in bounds + */ + bool isArcInBounds(const XYZCoord& center, double radius, double start_angle, double sweep); + /** * Generate a random point in a valid region or mapping region * @@ -99,6 +124,22 @@ namespace Environment { */ bool isPointInPolygon(const Polygon& polygon, const XYZCoord& point); + /** + * Determines whether a polygon lies entirely inside another one + * + * Every corner of the inner polygon has to be inside the outer one, and no + * edge of it may cross the outer boundary -- corners alone are not enough, + * as an edge can bulge out between two corners that are both inside. + * + * Points on the edge of the outer polygon count as outside, the same way + * isPointInPolygon treats them. + * + * @param inner ==> the polygon that has to be contained + * @param outer ==> the polygon that has to contain it + * @return ==> whether or not inner lies entirely inside outer + */ + bool isPolygonInPolygon(const Polygon& inner, const Polygon& outer); + /** * Checks wheter a line segment is in bounds or not, it must NOT intersect * either the valid region or the obstacles @@ -121,6 +162,38 @@ namespace Environment { bool doesLineIntersectPolygon(const XYZCoord& start_point, const XYZCoord& end_point, const Polygon& polygon); + /** + * Determines whether a circular arc intersects any edge of the polygon + * + * @param[in] center ==> the center of the arc's circle + * @param[in] radius ==> the radius of the arc's circle + * @param[in] start_angle ==> angle from the center to the arc's first + * point (0 is +x, CCW positive) + * @param[in] sweep ==> signed angle swept, CCW positive + * @param[in] polygon ==> polygon to check + * @return ==> true if arc intersects edge + */ + bool doesArcIntersectPolygon(const XYZCoord& center, double radius, double start_angle, + double sweep, const Polygon& polygon); + + /** + * Determines whether a circular arc intersects a line segment + * + * Intersects the segment with the full circle then checks whether each hit + * lies within both the segment and the arc's angular range. + * + * @param[in] center ==> the center of the arc's circle + * @param[in] radius ==> the radius of the arc's circle + * @param[in] start_angle ==> angle from the center to the arc's first + * point (0 is +x, CCW positive) + * @param[in] sweep ==> signed angle swept, CCW positive + * @param[in] seg_start ==> start point of the segment + * @param[in] seg_end ==> end point of the segment + * @return if arc intersecs a line segement + */ + bool doesArcIntersectSegment(const XYZCoord& center, double radius, double start_angle, + double sweep, const XYZCoord& seg_start, const XYZCoord& seg_end); + /** * Given three colinear points p, q, r, the function checks if * point q lies on line segment 'pr' @@ -229,15 +302,6 @@ namespace Environment { const XYZCoord& start_point, const XYZCoord& end_point); - /** - * Estimate the area and path length covered by the given goals - * - * @param goals the new goals - * - * @return a pair of the area covered and the path length - */ - std::pair estimateAreaCoveredAndPathLength(const std::vector& goals); - /** * Returns a new polygon that is scaled by a given factor * diff --git a/include/pathing/rrt.hpp b/include/pathing/rrt.hpp new file mode 100644 index 00000000..0955179f --- /dev/null +++ b/include/pathing/rrt.hpp @@ -0,0 +1,255 @@ +#ifndef INCLUDE_PATHING_RRT_HPP_ +#define INCLUDE_PATHING_RRT_HPP_ + +#include +#include +#include +#include + +#include "pathing/dubins.hpp" +#include "pathing/tree.hpp" +#include "utilities/constants.hpp" +#include "utilities/datatypes.hpp" + +// the different of final approaches to the goal +// yes, this is the default unit circle diagram used in High-School +inline const std::vector DEFAULT_GOAL_ANGLES = { + 0, + M_PI / 6, + M_PI / 4, + M_PI / 3, + M_PI / 2, + 2 * M_PI / 3, + 3 * M_PI / 4, + 5 * M_PI / 6, + M_PI, + 7 * M_PI / 6, + 5 * M_PI / 4, + 4 * M_PI / 3, + 3 * M_PI / 2, + 5 * M_PI / 3, + 7 * M_PI / 4, + 11 * M_PI / 6, +}; + +/** + * A candidate flight from a node already in the tree to some point. + * + * The tree only stores nodes that are connected to it, so a connection that has + * not been committed yet is described by the anchor it would hang off of and the + * dubins path that gets there. The point may be a sample or a goal, the search + * that finds it does not care which. + */ +struct Connection { + NodeId anchor = INVALID_NODE; // node in the tree the path starts at + RRTPoint end{}; // the vector the path lands on + RRTOption option{}; // dubins path anchor --> end + double cost = std::numeric_limits::infinity(); // path length from the root to the end + + inline bool isValid() const { return anchor != INVALID_NODE; } +}; + +/** + * One leg of the mission, flown from a waypoint to the one behind it. + * + * The tree that found the leg is thrown away as soon as the waypoint is reached, + * so what it settled on is kept here instead. That is enough to say how long the + * leg is, which is all a caller comparing two missions needs -- the points along + * it are not worth flying until one of them has been picked. + */ +struct Leg { + RRTPoint start; // the waypoint the leg is flown from + std::vector segments; // the dubins paths flown, in order + double length = 0; // the ground the leg covers + int goal_idx = 0; // index of the goal the leg lands on +}; + +class RRT { + public: + // tree stores the nodes that form the tree + RRTTree tree; + + /* + * The waypoints to path through, in order, the first of which is where the + * plane starts out. A leg is always flown from the waypoint behind it, so + * the start being one of them is what keeps the first leg from being a + * special case. + */ + const std::vector goals; + + /* + * The final approach angles each goal may be reached at, one set per goal. + * The cheapest of them wins, so a caller that does not care which way a goal + * is approached hands over every angle, and one that does -- coverage pathing + * does, as a scan line only sweeps the ground it is meant to if it is flown + * along its own direction -- hands over the single angle it wants. + * + * The first set is the heading the plane is already flying, as the goal it is + * sitting on is flown from rather than reached. + */ + const std::vector> goal_angles; + + // the legs of the mission that have been searched out so far + std::vector legs; + + // the points flown along those legs, once they have been generated + std::vector flight_path; + + /* + * Scratch space for bestConnection, which runs once for every sample RRT takes + * and would otherwise lay all of this out again each time. It is not state -- + * nothing may read it between calls, and every call fills it before it reads it. + */ + mutable std::array bounds; // by node, the cheapest flight through it + mutable std::array frontier; // the order the nodes are looked at in + mutable std::vector options; // the paths out of the node being expanded + + + /** + * @param[in] goals ==> the waypoints to fly through, in order, the + * first of which is where the plane already is + * @param[in] start_angle ==> the heading the plane is flying at right now + * @param[in] goal_angles ==> the angles each goal may be approached at, one + * set per goal. A goal that has to be flown at + * one particular heading is a set of one. The + * set for the first goal is not read, the plane + * is already sitting on it at start_angle. + */ + RRT(std::vector goals, double start_angle, + std::vector> goal_angles); + + /** + * @param[in] goals ==> the waypoints to fly through, in order, the + * first of which is where the plane already is + * @param[in] start_angle ==> the heading the plane is flying at right now + * @param[in] angles ==> the angles every goal may be approached at + */ + RRT(std::vector goals, double start_angle, std::vector angles = {}); + + /** + * RRT algorithm -- searches out the mission and then flies it + */ + void run(); + + /** + * Searches out the dubins paths that fly the mission, and nothing more + * + * How long the mission is falls out of this, so a caller weighing one against + * another can stop here and only pay for the points of the one it flies. + * + * TODO - do all iterations to try to find the most efficient path? + * - maybe do the tolarance as stright distance / num iterations + * - not literally that function, but something that gets more leniant the + * more iterations there are + */ + void generateDubinsOptions(); + + /** + * Flies the legs, which is the only thing that generates points + */ + void generateFlightPoints(); + + /** + * The ground the legs found so far cover + * + * Available as soon as the dubins paths are, the points do not have to have + * been generated. + * + * @return ==> the length of every leg flown, added up + */ + double pathLength() const; + + /** + * returns a continuous path of points to the goal + * + * @return ==> list of 2-vectors to the goal region + */ + std::vector getPointsToGoal() const; + + /** + * Does a single iteration of the RRT(star) algoritm to connect two waypoints + * + * @return ==> whether or not the goal was reached + */ + bool RRTIteration(uint8_t cur_goal_idx); + + /** + * The cheapest a flight through a node could possibly be + * + * A dubins path is never shorter than the straight line between the two + * points it connects, so the flight has to cover at least the distance + * already flown to reach the node plus that straight line. + * + * @param[in] node ==> node in the tree the flight would go through + * @param[in] ends ==> the points being pathed to + * @return ==> lower bound on the cost of any connection from the node + */ + double lowerBound(NodeId node, const std::vector &ends) const; + + /** + * Appends every dubins path that exists from a single node to each of the + * given points + * + * @param[in] node ==> node in the tree to path from + * @param[in] ends ==> the points to path to + */ + void fillOptions(NodeId node, const std::vector &ends) const; + + /** + * Finds the cheapest flyable connection from the tree to any of the given + * points, which is NOT added into the tree + * + * Nodes are visited in order of the cheapest flight they could possibly + * offer, so the search stops as soon as that bound passes the best + * connection it already holds -- the rest of the tree cannot beat it, and + * pathing from it would be wasted work. + * + * @param[in] ends ==> the points to path to + * @param[in] max_paths_checked ==> how many paths may be checked before + * giving up + * @return ==> the connection if one was found, an invalid connection + * otherwise + */ + Connection bestConnection(const std::vector &ends) const; + + /** + * The points a goal can be reached at, one for every angle it may be + * approached at -- which is a single point when the caller pinned it down + * + * @param[in] cur_goal_idx ==> index of the goal that we are trying to + * connect to + * @return ==> the goal, at each of the approach angles + */ + std::vector goalEndpoints(int cur_goal_idx) const; + + /** + * Connects to the goal after RRT is finished + * + * @param[in] cur_goal_idx ==> index of the goal that we are trying to + * connect to + * @return ==> whether or not the goal was connected to + */ + bool connectToGoal(int cur_goal_idx); + + /** + * Does the logistical work when one waypoint is reached from another + * - adds the node to the tree + * - keeps the dubins paths that got there, which outlive the tree + * - resets the tree with the goal as its new root + * + * @param[in] connection ==> the connection to the goal to commit + * @param[in] cur_goal_idx ==> index of the goal that we are trying to connect to + */ + void commitConnection(const Connection &connection, int cur_goal_idx); + + /** + * The points flown along one leg, climbing from the altitude of the waypoint + * behind it to the one it lands on + * + * @param[in] leg ==> the leg to fly + * @return ==> the points along the leg, at altitude + */ + std::vector buildFlightPath(const Leg &leg) const; +}; + +#endif // INCLUDE_PATHING_RRT_HPP_ diff --git a/include/pathing/static.hpp b/include/pathing/static.hpp index 4a1669a0..63c593b9 100644 --- a/include/pathing/static.hpp +++ b/include/pathing/static.hpp @@ -15,182 +15,21 @@ #include "pathing/environment.hpp" #include "pathing/mission_path.hpp" #include "pathing/plotting.hpp" +#include "pathing/rrt.hpp" #include "pathing/tree.hpp" #include "utilities/constants.hpp" #include "utilities/datatypes.hpp" #include "utilities/rng.hpp" -class RRT { - public: - RRT(RRTPoint start, std::vector goals, double search_radius, - const OBCConfig &config, std::vector angles = {}); - - /** - * RRT(-star) algorithm - * - * TODO - do all iterations to try to find the most efficient path? - * - maybe do the tolarance as stright distance / num iterations - * - not literally that function, but something that gets more leniant the - * more iterations there are - */ - void run(); - - /** - * returns a continuous path of points to the goal - * - * @return ==> list of 2-vectors to the goal region - */ - std::vector getPointsToGoal() const; - - private: - // tree stores the nodes that form the tree - RRTTree tree; - - const std::vector goals; // the waypoints to path through, in order - - /* RRT Config Options */ - const int iterations_per_waypoint; // number of times to run the RRT algorithm - // for each waypoint - const double search_radius; // !!NOT USED!! max radius to move off the tree - const double rewire_radius; // ONLY FOR RRT-STAR, max radius from new node to rewire - const RRTConfig config; // optimization options - std::vector flight_path; - - // the different of final approaches to the goal - // yes, this is the default unit circle diagram used in High-School - std::vector angles = { - 0, - M_PI / 6, - M_PI / 4, - M_PI / 3, - M_PI / 2, - 2 * M_PI / 3, - 3 * M_PI / 4, - 5 * M_PI / 6, - M_PI, - 7 * M_PI / 6, - 5 * M_PI / 4, - 4 * M_PI / 3, - 3 * M_PI / 2, - 5 * M_PI / 3, - 7 * M_PI / 4, - 11 * M_PI / 6, - }; - - /** - * Does a single iteration of the RRT(star) algoritm to connect two waypoints - * - * @param tries ==> number of points it attempts to sample - * @return ==> whether or not the goal was reached - */ - bool RRTIteration(int tries, int current_goal_index); - - /** - * Evaluates a certain interval to determine if the algorithm is making - * meaningful progress. If it isn't, it will simply tell the RRT algoritm to - * stop. - * - * @param goal_node ==> current best node reaching the goal (updated if better found) - * @param goal_parent ==> parent of the goal_node (updated if better found) - * @param current_goal_index ==> index of the goal that we are trying to connect to - * @return ==> true if the RRT algorithm should stop (converged or - * adequate), false otherwise - */ - bool epochEvaluation(std::shared_ptr goal_node, - std::shared_ptr goal_parent, - int current_goal_index); - - /** - * Generates a random point in the airspace (uniformly) - * - * @return ==> random point in the airspace - */ - RRTPoint generateSamplePoint() const; - - /** - * Gets a sorted list of options to EACH one of the possible goals, defined - * by the angles we want to connect to - * - * @param current_goal_index ==> index of the goal that we are trying to - * connect to - * @param total_options ==> number of options to try to connect to the goal - * @return ==> list of options to connect to the goal - * - */ - std::vector, RRTOption>>> - getOptionsToGoal(int current_goal_index, int total_options) const; - - /** - * Tries to get the optimal node to the goal, which is NOT connected into the - * tree - * - * @param current_goal_index ==> index of the goal that we are trying to - * connect to - * @param total_options ==> number of options to try to connect to the goal - * @param parent ==> pointer to the parent node (output parameter) - * @return ==> pointer to the node if one was found, - * nullptr otherwise - */ - std::shared_ptr sampleToGoal(int current_goal_index, - int total_options, - std::shared_ptr& parent) const; - - /** - * Connects to the goal after RRT is finished - * - * @param current_goal_index ==> index of the goal that we are trying to - * connect to - * @param total_options ==> number of options to try to connect to the goal - * @return ==> pointer to the node if it was added, - * nullptr otherwise - */ - bool connectToGoal(int current_goal_index, - int total_options = TOTAL_OPTIONS_FOR_GOAL_CONNECTION); - - /** - * Does the logistical work when found one waypoint to another - * - adds the node to the tree - * - finds the path - * - adds altitude to the path - * - * @param goal_node ==> node to add to the tree - * @param parent ==> parent of the goal node - * @param current_goal_index ==> index of the goal that we are trying to - */ - void addNodeToTree(std::shared_ptr goal_node, - std::shared_ptr parent, - int current_goal_index); - - /** - * Goes through generated options to try to connect the sample to the tree - * - * @param options ==> list of options to connect the sample to the tree - * @param sample ==> sampled point - * @return ==> whether or not the sample was successfully added to - * the tree (nullptr if not added) - */ - std::shared_ptr parseOptions( - const std::vector, RRTOption>> &options, - const RRTPoint &sample); - - /** - * Rewires the tree by finding paths that are more efficintly routed through - * the sample. Only searches for nodes a specific radius around the sample - * to reduce computational expense - * - * @param sample ==> sampled point - */ - void optimizeTree(std::shared_ptr sample); -}; /** * Class that performs Coverage-Path_Planning (CPP) over a given polygon * - * Basically draws vertical lines, and the connects them with Dubins paths + * Basically draws vertical lines, and then connects them with RRT, which keeps + * the legs between the lines inside the airspace * * Limitations * - Cannot path through non-convex shapes - * - Does not check if path is inbounds or not * * Notes: * - this implementation is for fixed wing planes, which is not currently being used. However, @@ -221,14 +60,30 @@ class ForwardCoveragePathing { std::vector coverageOptimal() const; /** - * From a list of dubins paths and waypoints, generate a path + * The waypoints that sweep the zone with one layout of scan lines, starting + * from where the plane is now + * + * These are the mission, not the path -- each one carries the heading its + * line has to be flown at, and RRT is what works out how to get from one to + * the next. + * + * @param one_way ==> whether every line is flown in the same direction, + * rather than alternating + * @param vertical ==> whether the lines run vertically + */ + std::vector scanLines(bool one_way, bool vertical) const; + + /** + * Searches out the dubins paths that fly one layout of scan lines * - * @param dubins_options ==> list of dubins options to connect the waypoints - * @param waypoints ==> list of waypoints to connect (always 1 more element than - * dubins_options) + * The points along them are not generated, so the caller may weigh the + * mission against another one and throw it away cheaply. + * + * @param one_way ==> whether every line is flown in the same direction, + * rather than alternating + * @param vertical ==> whether the lines run vertically */ - std::vector generatePath(const std::vector &dubins_options, - const std::vector &waypoints) const; + RRT pathScanLines(bool one_way, bool vertical) const; private: const double scan_radius; // how far each side of the plane we intend to look (half dist @@ -313,9 +168,4 @@ generateSearchPath(std::shared_ptr state, double start_angle); std::vector generateAirdropApproach(std::shared_ptr state, const GPSCoord &goal); -std::vector> generateGoalListDeviations(const std::vector &goals, - XYZCoord deviation_point); - -std::vector> generateRankedNewGoalsList(const std::vector &goals); - #endif // INCLUDE_PATHING_STATIC_HPP_ diff --git a/include/pathing/tree.hpp b/include/pathing/tree.hpp index 28786ef7..7f66a492 100644 --- a/include/pathing/tree.hpp +++ b/include/pathing/tree.hpp @@ -1,339 +1,76 @@ #ifndef INCLUDE_PATHING_TREE_HPP_ #define INCLUDE_PATHING_TREE_HPP_ -#include -#include -#include -#include -#include +#include +#include #include #include "pathing/dubins.hpp" +#include "utilities/constants.hpp" #include "utilities/datatypes.hpp" -#include "utilities/obc_config.hpp" -#include "utilities/rng.hpp" -class RRTNode; -typedef std::vector> RRTNodeList; -typedef XYZCoord Vector; +using NodeId = uint16_t; +constexpr NodeId INVALID_NODE = 0xFFFF; // HARDCODED FOR uint16_t -class RRTNode { - public: - RRTNode(const RRTPoint& point, double cost, double path_length, - const std::vector path); - RRTNode(const RRTPoint& point, double cost, double path_length, - const std::vector path, RRTNodeList reachable); - - - /* - * Equality overload method for RRTNode object - */ - bool operator==(const RRTNode& other_node) const; - - /* - * Get the RRTPoint associated with this RRTNode object - */ - RRTPoint& getPoint(); - - /* - * Set the reachable (neighbors) list for this RRTNode object - */ - void setReachable(const RRTNodeList& reachable); - - /* - * Add a new node to the end of this node's reachable list. - */ - void addReachable(std::shared_ptr new_node); +template +struct TreeArray { + static_assert(N < INVALID_NODE, "TREE is TOO LARGE"); - /* - * Remove a specific node from this node's reachable list. - */ - void removeReachable(std::shared_ptr old_node); + std::array points; + std::array rrt_options; + std::array length; + std::array parent; + std::array first_child; // INVALID_NODE if leaf + std::array next_sibling; // INVALID_NODE if last + NodeId size = 0; - /* - * Return a reference to this node's reachable list - */ - const RRTNodeList& getReachable(); - - /* - * Get the cost associated with this node - */ - double getCost() const; - - /* - * Set the cost associated with this node - */ - void setCost(double new_cost); - - /** - * Get the path associated with this node - * - * @return std::vector path - */ - const std::vector& getPath() const; - - /** - * Set the path associated with this node - * - * @param path std::vector path - */ - void setPath(const std::vector& path); - - /** - * Get the path length associated with this node - * - * @return double path length - */ - double getPathLength() const; - - /** - * Set the path length associated with this node - * - * @param path_length double path length - */ - void setPathLength(double path_length); - - private: - RRTPoint point; - RRTNodeList reachable{}; - double cost; - double path_length; - std::vector path{}; + NodeId alloc() { return size++; } + void reset() { size = 0; } }; class RRTTree { public: - explicit RRTTree(RRTPoint root_point); - ~RRTTree(); - - /** - * Generates node without adding it to the tree - */ - std::shared_ptr generateNode(std::shared_ptr anchor_node, - const RRTPoint& new_point, - const RRTOption& option) const; - - /** - * Adds a node to the tree - * - * @param anchor_node ==> the node to connect to - * @param new_node ==> the node to add - */ - bool addNode(std::shared_ptr anchor_node, std::shared_ptr new_node); - - /* - * Add a node to the RRTTree. - * If adding the first node to the tree, connectTo can be anything. - */ - std::shared_ptr addSample(std::shared_ptr anchor_node, - const RRTPoint& new_point, - const RRTOption& option); - - /** - * Returns a pointer to the root node - * - * @return std::shared_ptr pointer to root node - */ - std::shared_ptr getRoot() const; + TreeArray tree; - bool validatePath(const std::vector& path, const RRTOption& options) const; + explicit RRTTree(RRTPoint root_point); /** - * Returns a sorted list of the paths to get from a given node to the sampled - * node - * - * @param end ==> the sampled node that needs to be connected - * to the tree - * @param quantity_options ==> the number of results to return back to the - * function - * @return ==> mininum sorted list of pairs of - */ - std::vector, RRTOption>> pathingOptions( - const RRTPoint& end, PointFetchMethod::Enum path_option = PointFetchMethod::Enum::NONE, - int quantity_options = MAX_DUBINS_OPTIONS_TO_PARSE) const; - - /** DOES RRT* for the program + * Add a node to the RRTTree. ASSUMES ROOT EXISTS * - * @param sample ==> the point to used as the base - * @param rewire_radius ==> the radius to search for nodes to rewire + * @param[in] parent ==> The parent of the new node + * @param[in] new_point ==> RRTPoint for the new node + * @param[in] option ==> RRTOption for the new node */ - void RRTStar(std::shared_ptr sample, double rewire_radius); + void addSample(NodeId parent, const RRTPoint new_point, const RRTOption option); /** * Changes the currentHead to the given goal * - * @param goal ==> the goal to change the currentHead to - */ - void setCurrentHead(std::shared_ptr goal); - - /** - * Rewires an edge from an old path to a new path. - * preserves ALL elements of the tree (i.e. NO elements are removed). - * - * @param current_point ==> the current/end point to be rewired - * @param previous_parent ==> the previous parent to the current point - * @param new_parent ==> the new parent to the current point - * @param path ==> the new path new_parrent --> current_point - * @param path_cost ==> the cost of the new path - */ - void rewireEdge(std::shared_ptr current_point, - std::shared_ptr previous_parent, - std::shared_ptr new_parent, - const std::vector& path, double path_cost); - - /** - * Gets K random nodes from the tree, starting at the current head - * - * @param k ==> the number of nodes to get - * @return ==> list of k random nodes (unordered) - */ - std::vector> getKRandomNodes(int k) const; - - /** - * __Recursive Helper__ - * Gets K random nodes from the tree, starting at the current head - * - * @param nodes ==> the list (reference) of nodes to add to - * @param current_node ==> the current node that is being accessed - * @param k ==> the number of nodes to get - * @param chance ==> the chance to add the current node to the list - */ - void getKRandomNodesRecursive(std::vector>& nodes, - std::shared_ptr current_node, - double chance) const; - - /** - * Gets the k closest nodes to a given point - * - * @param sample ==> the point to find the closest nodes to - * @param k ==> the number of nodes to get - * @return ==> list (ordered) of k closest nodes + * @param[in] goal ==> the goal to change the currentHead to */ - std::vector> getKClosestNodes(const RRTPoint& sample, int k) const; - - /** - * __Recursive Helper__ - * Gets the k closest nodes to a given point - * - * @param nodes_by_distance ==> the list (reference) of {distance, node} to add to - * @param sample ==> the point to find the closest nodes to - * @param current_node ==> the current node that is being accessed - */ - void getKClosestNodesRecursive(std::vector>>& nodes_by_distance, - const RRTPoint& sample, - std::shared_ptr current_node) const; - - /** - * Fills in a list of options from an existing list of nodes - * - * @param options ==> the list of options to fill - * @param nodes ==> the list of nodes to parse - * @param sample ==> the end point that the options will be connected to - */ - void fillOptionsNodes(std::vector, - RRTOption>>& options, - const std::vector>& nodes, - const RRTPoint& sample) const; - - /** - * Returns the segment of path from the given node to the current head - * - * @param node ==> the node to start the path from - * @return ==> the path from the node to the current head - */ - std::vector getPathSegment(std::shared_ptr node) const; + void setCurrentHead(RRTPoint goal); /** * Returns the start RRTPoint * * @return RRTPoint start point */ - RRTPoint& getStart() const; - - private: - std::shared_ptr root; - std::shared_ptr current_head; - int tree_size; + RRTPoint getStart() const; /** - * Helper that deletes the tree + * Finds the sequence of dubins paths flown from current_head to the target node * - * @param node ==> the root of the tree to delete - */ - void deleteTree(std::shared_ptr node); - - /** - * traverses the tree, and puts in all RRTOptions from dubins into a list - * (DFS) - * - * @param options ==> The list of options that is meant to be filled - * @param node ==> current node that will be traversed (DFS) - * @param sample ==> the end point that the options will be connected to - */ - void fillOptions(std::vector, RRTOption>>& options, - std::shared_ptr node, - const RRTPoint& sample) const; - - /** - * Gets the nearest node to a given RRTPoint NOT USED AT THE MOMENT - * - * @param point ==> the point to find the nearest node to - * @return ==> the nearest node to the point - */ - // std::pair, double> getNearestNode(const XYZCoord& point) const; - - /** - * RRTStar Recursive - * (RECURSIVE HELPER) - * Rewires the tree by finding paths that are more efficintly routed through - * the sample. Only searches for nodes a specific radius around the sample - * to reduce computational expense - * - * @param current_node ==> current node (DFS) - * @param sample ==> sampled point - * @param search_radius ==> the radius to search for nodes to rewire - */ - void RRTStarRecursive(std::shared_ptr current_node, - std::shared_ptr sample, - double rewire_radius); - - /** - * After rewire edge, it goes down the tree and reassigns the cost of the - * nodes + * Every node stores the path its parent takes to reach it, and knows its + * parent, so the path is walked up to the root instead of searched down from + * it. The root itself contributes nothing, as it is where the path starts. * - * @param changed_node the node that has been changed - */ - void reassignCosts(std::shared_ptr changed_node); - - /** - * Recurses down the tree to reassign the costs of the nodes - * (RECURSIVE HELPER) - * - * @param parent ==> the parent node - * @param node ==> the current node - * @param path_cost ==> the cost of the path to the current node - */ - void reassignCostsRecursive(std::shared_ptr parent, - std::shared_ptr current_node, - double cost_difference); - - /** - * Finds the sequence of nodes from current_head to the target node using BFS - * - * @param target_node ==> the node to find the path to - * @return ==> vector of nodes from current_head to target_node - */ - RRTNodeList findPathToNode(std::shared_ptr target_node) const; - - /** - * Constructs the coordinate path from a sequence of nodes + * A node's point is where the path stored on it lands, so the two travel + * together and the vectors never have to be recomputed downstream. * - * @param nodes ==> sequence of nodes - * @return ==> vector of coordinates representing the path + * @param[in] target_node ==> the node to find the path to + * @return ==> segments from current_head to target_node, in flight order */ - std::vector buildPathFromNodes( - const std::vector>& nodes) const; + std::vector findPathToNode(NodeId target_node) const; }; #endif // INCLUDE_PATHING_TREE_HPP_ diff --git a/include/utilities/constants.hpp b/include/utilities/constants.hpp index bae83fe5..6f7c6137 100644 --- a/include/utilities/constants.hpp +++ b/include/utilities/constants.hpp @@ -20,17 +20,14 @@ const double TWO_PI = 2 * M_PI; const double HALF_PI = M_PI / 2; // RRT CONSTANTS -const int ITERATIONS_PER_WAYPOINT = 256; // number of times RRT is ran per waypoint -const double SEARCH_RADIUS = - 1000.0; // DOES NOTHING, limits how far off the tree the new node can be +const int TREE_CAPACITY = 512; // number of nodes a tree between two waypoints is built out of +// the tree holds the waypoint it is rooted at, a node for every sample RRT takes, +// and the waypoint they end up connecting to +const int ITERATIONS_PER_WAYPOINT = TREE_CAPACITY - 2; // number of times RRT is ran per waypoint const double REWIRE_RADIUS = 200.0; // ONLY FOR RRT-STAR, max radius from new node to rewire // RRT HELPER CONSTANTS -const double EPOCH_TEST_MARGIN = 0.97; // at what margin of improvement does it stop const int ENV_PATH_VALIDATION_STEP_SIZE = 5; // how many points to skip when validating path -const int NUM_EPOCHS = 5; // number of times to evaulate the path length -const int K_RANDOM_NODES = 100; // how many nodes to generate for the tree -const int K_CLOESEST_NODES = 50; // how many nodes to look at when finding the closest node const int TOTAL_OPTIONS_FOR_GOAL_CONNECTION = 2048; // TODO - MUST SCALE WITH ITERATIONS OR ELSE CANT FIND GOAL diff --git a/include/utilities/datatypes.hpp b/include/utilities/datatypes.hpp index a158adcb..6b8f10ea 100644 --- a/include/utilities/datatypes.hpp +++ b/include/utilities/datatypes.hpp @@ -13,6 +13,8 @@ #include "utilities/jsonable.hpp" struct XYZCoord: jsonable{ + // members left indeterminate; only needed so this can live in a std::array + XYZCoord() = default; XYZCoord(double x, double y, double z) : x(x), y(y), z(z) {} /** @@ -63,6 +65,8 @@ struct XYZCoord: jsonable{ }; struct RRTPoint { + // members left indeterminate; only needed so this can live in a std::array + RRTPoint() = default; RRTPoint(XYZCoord point, double psi); /* * Equality overload method for RRTPoint diff --git a/include/utilities/obc_config.hpp b/include/utilities/obc_config.hpp index f986cd8f..17ca06a8 100644 --- a/include/utilities/obc_config.hpp +++ b/include/utilities/obc_config.hpp @@ -44,32 +44,11 @@ struct CVConfig { uint16_t not_stolen_port; }; -namespace PointFetchMethod { -enum class Enum { - NONE, // check RRT against every node (path optimal, but incredibly slow) - RANDOM, // check ~k randomly sampled nodes from the tree. - NEAREST // check ~$p$ nodes closest to the sampled node (best performance/time ratio from - // rudimentary testing) -}; -CONFIG_VARIANT_MAPPING_T(Enum) -MAPPINGS = {{"none", Enum::NONE}, {"random", Enum::RANDOM}, {"nearest", Enum::NEAREST}}; -}; // namespace PointFetchMethod - struct DubinsConfig { double turning_radius; double point_separation; }; -struct RRTConfig { - int iterations_per_waypoint; // number of iterations run between two waypoints - double rewire_radius; // maximum distance from sampled point to optimize during RRT* - bool optimize; // run RRT* if true - PointFetchMethod::Enum point_fetch_method; - bool allowed_to_skip_waypoints; // if true, will skip waypoints if it can not connect after 1 - // RRT iteration - bool generate_deviations; -}; - namespace AirdropCoverageMethod { enum class Enum { HOVER, FORWARD }; CONFIG_VARIANT_MAPPING_T(Enum) MAPPINGS = {{"hover", Enum::HOVER}, {"forward", Enum::FORWARD}}; @@ -108,7 +87,6 @@ struct PathingConfig { int laps; double upload_distance_buffer_m; DubinsConfig dubins; - RRTConfig rrt; AirdropCoverageConfig coverage; AirdropApproachConfig approach; }; diff --git a/src/core/mission_parameters.cpp b/src/core/mission_parameters.cpp index 1d4b3bd1..5a8216f7 100644 --- a/src/core/mission_parameters.cpp +++ b/src/core/mission_parameters.cpp @@ -97,10 +97,31 @@ std::optional MissionParameters::setMission( return err; } + const Polygon flight_boundary = cconverter.toXYZ(mission.flightboundary()); + const Polygon airdrop_boundary = cconverter.toXYZ(mission.airdropboundary()); + const Polyline mission_waypoints = cconverter.toXYZ(mission.waypoints()); + + // nothing outside of the flight boundary can be flown to, so a mission that + // asks for it is never going to be one the plane can fly + for (std::size_t i = 0; i < mission_waypoints.size(); i++) { + if (!Environment::isPointInPolygon(flight_boundary, mission_waypoints[i])) { + err += "Waypoint " + std::to_string(i + 1) + " is outside of the flight boundary. "; + } + } + + // the airdrop boundary doubles as the mapping region + if (!Environment::isPolygonInPolygon(airdrop_boundary, flight_boundary)) { + err += "Airdrop boundary is not entirely inside of the flight boundary. "; + } + + if (!err.empty()) { + return err; + } + this->cached_mission = mission; - this->flightBoundary = cconverter.toXYZ(mission.flightboundary()); - this->airdropBoundary = cconverter.toXYZ(mission.airdropboundary()); - this->waypoints = cconverter.toXYZ(mission.waypoints()); + this->flightBoundary = flight_boundary; + this->airdropBoundary = airdrop_boundary; + this->waypoints = mission_waypoints; for (const auto& airdrop : mission.airdropassignments()) { // Use const& for efficiency this->_setAirdrop(airdrop); } diff --git a/src/network/gcs_routes.cpp b/src/network/gcs_routes.cpp index c4d1278a..13efb64f 100644 --- a/src/network/gcs_routes.cpp +++ b/src/network/gcs_routes.cpp @@ -16,6 +16,7 @@ #include "core/mission_state.hpp" #include "network/gcs_macros.hpp" #include "network/mavlink.hpp" +#include "pathing/environment.hpp" #include "pathing/mission_path.hpp" #include "protos/obc.pb.h" #include "ticks/airdrop_approach.hpp" @@ -148,13 +149,26 @@ DEF_GCS_HANDLE(Post, targets, locations) { curr_alt_m = state->getMav()->altitude_msl_m(); } + const std::optional>& converter = + state->getCartesianConverter(); + if (!converter.has_value()) { + LOG_RESPONSE(ERROR, "No mission uploaded to check the drop locations against", BAD_REQUEST); + return; + } + const Polygon flight_boundary = state->mission_params.getFlightBoundary(); + nlohmann::json waypoints = nlohmann::json::parse(request.body); AirdropTarget airdrop_target; if (!waypoints.is_array()) { LOG_RESPONSE(ERROR, "Waypoints is not a vactor", BAD_REQUEST); + return; } + // nothing is sent until every target has been checked, so a bad one does not + // leave the plane holding half of an upload + std::vector> drops; + for (const auto& waypoint : waypoints) { google::protobuf::util::JsonStringToMessage(waypoint.dump(), &airdrop_target); @@ -168,8 +182,19 @@ DEF_GCS_HANDLE(Post, targets, locations) { return; } - float drop_lat = airdrop_target.coordinate().latitude(); - float drop_lng = airdrop_target.coordinate().longitude(); + // the plane cannot fly to a drop it is not allowed to fly to + if (!Environment::isPointInPolygon(flight_boundary, + converter->toXYZ(airdrop_target.coordinate()))) { + LOG_RESPONSE(ERROR, "Drop location is outside of the flight boundary", BAD_REQUEST); + return; + } + + drops.push_back({airdrop, airdrop_target.coordinate()}); + } + + for (const auto& [airdrop, coordinate] : drops) { + float drop_lat = coordinate.latitude(); + float drop_lng = coordinate.longitude(); state->getAirdrop()->send(makeLatLngPacket(SEND_LATLNG, airdrop, TARGET_ACQUIRED, drop_lat, drop_lng, curr_alt_m)); } diff --git a/src/pathing/CMakeLists.txt b/src/pathing/CMakeLists.txt index 97bf7d8e..5ced9487 100644 --- a/src/pathing/CMakeLists.txt +++ b/src/pathing/CMakeLists.txt @@ -4,6 +4,7 @@ set(FILES dubins.cpp environment.cpp plotting.cpp + rrt.cpp static.cpp tree.cpp mission_path.cpp diff --git a/src/pathing/dubins.cpp b/src/pathing/dubins.cpp index 1807d3e4..ffb19d24 100644 --- a/src/pathing/dubins.cpp +++ b/src/pathing/dubins.cpp @@ -111,12 +111,15 @@ std::vector Dubins::generatePointsStraight(const RRTPoint& start, cons 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); + + // the straightaway is only described by the two points it runs between, so that ardupilot + // accelerates through it instead of slowing down for every point along the way + bool straight_added = false; + for (double current_distance = 0; current_distance < total_distance; current_distance += _point_separation) { if (current_distance < std::abs(path.beta_0) * _radius) { // First turn @@ -125,19 +128,19 @@ std::vector Dubins::generatePointsStraight(const RRTPoint& start, cons 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); + } else if (!straight_added) { // Straignt Section + points_list.emplace_back(initial_terminal_point); + points_list.emplace_back(final_terminal_point); + straight_added = true; } } - points_list.emplace_back(XYZCoord{end.coord.x, end.coord.y, 0}); + + const XYZCoord end_point{end.coord.x, end.coord.y, 0}; + + // the last turn may have already ended on the end point + if (points_list.empty() || points_list.back().distanceTo(end_point) > 0) { + points_list.emplace_back(end_point); + } return points_list; } @@ -206,6 +209,27 @@ std::vector Dubins::generatePoints(const RRTPoint& start, const RRTPoi return generatePointsCurve(start, end, path); } +std::vector Dubins::generatePath(const RRTPoint& start, + const std::vector& segments) { + std::vector path; + RRTPoint current = start; + + for (const PathSegment& segment : segments) { + const std::vector& points = + generatePoints(current, segment.end, segment.option.dubins_path, + segment.option.has_straight); + + // the first point of the segment is where the previous one ended + if (!points.empty()) { + path.insert(path.end(), points.begin() + 1, points.end()); + } + + current = segment.end; + } + + return 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); diff --git a/src/pathing/environment.cpp b/src/pathing/environment.cpp index 54d74968..77e3af20 100644 --- a/src/pathing/environment.cpp +++ b/src/pathing/environment.cpp @@ -2,6 +2,7 @@ #include #include +#include #include #include @@ -76,6 +77,85 @@ bool isPathInBounds(const std::vector& path) { return true; } +bool isDubinsPathInBounds(const RRTPoint& start, const RRTPoint& end, const RRTOption& option) { + // [LRL, RLR] are disabled in Dubins::allOptions(); + if (!option.has_straight) { + return false; + } + + // rejects the sentinel options (infinity) that lsr/rsl produce when no path exists + if (!std::isfinite(option.length)) { + return false; + } + + const double radius = Dubins::_radius; + const DubinsPath& path = option.dubins_path; + + if (!isPointInBounds(start.coord) || !isPointInBounds(end.coord)) { + return false; + } + + // endpoints of the straight section, overwritten by the turn sections below + XYZCoord straight_start = start.coord; + XYZCoord straight_end = end.coord; + + // first turn + if (std::abs(path.beta_0) > 0) { + const double turn_sign = (path.beta_0 > 0) ? 1.0 : -1.0; + const XYZCoord center = Dubins::findCenter(start, (turn_sign > 0) ? 'L' : 'R'); + + // angle from the center to the plane at the start of the turn + const double start_angle = start.psi - HALF_PI * turn_sign; + if (!isArcInBounds(center, radius, start_angle, path.beta_0)) { + return false; + } + + const double exit_angle = start.psi + (std::abs(path.beta_0) - HALF_PI) * turn_sign; + straight_start = + center + radius * XYZCoord{std::cos(exit_angle), std::sin(exit_angle), 0}; + } + + // last turn (entered backwards -- the arc runs from the end of the straight + // section to the end vector) + if (std::abs(path.beta_2) > 0) { + const double turn_sign = (path.beta_2 > 0) ? 1.0 : -1.0; + const XYZCoord center = Dubins::findCenter(end, (turn_sign > 0) ? 'L' : 'R'); + + // angle from the center to the plane at the start of the turn + const double entry_angle = end.psi - (std::abs(path.beta_2) + HALF_PI) * turn_sign; + if (!isArcInBounds(center, radius, entry_angle, path.beta_2)) { + return false; + } + + straight_end = + center + radius * XYZCoord{std::cos(entry_angle), std::sin(entry_angle), 0}; + } + + // straight section + return isLineInBounds(straight_start, straight_end); +} + +bool isArcInBounds(const XYZCoord& center, double radius, double start_angle, double sweep) { + // an arc that starts in bounds and never crosses a boundary is entirely in bounds + const XYZCoord arc_start = + center + radius * XYZCoord{std::cos(start_angle), std::sin(start_angle), 0}; + if (!isPointInBounds(arc_start)) { + return false; + } + + if (doesArcIntersectPolygon(center, radius, start_angle, sweep, _valid_region)) { + return false; + } + + for (const Polygon& obstacle : _obstacles) { + if (doesArcIntersectPolygon(center, radius, start_angle, sweep, obstacle)) { + return false; + } + } + + return true; +} + XYZCoord getRandomPoint(bool use_mapping_region, const XYZCoord& fallback) { // TODO - use some heuristic to more efficiently generate direction // vector (and make it toggleable) @@ -120,6 +200,24 @@ bool isPointInPolygon(const Polygon& polygon, const XYZCoord& point) { return is_inside; } +bool isPolygonInPolygon(const Polygon& inner, const Polygon& outer) { + for (const XYZCoord& corner : inner) { + if (!isPointInPolygon(outer, corner)) { + return false; + } + } + + // an edge can bulge out between two corners that are both inside, which + // shows up as it crossing the outer boundary + for (std::size_t i = 0, j = inner.size() - 1; i < inner.size(); j = i++) { + if (doesLineIntersectPolygon(inner[j], inner[i], outer)) { + return false; + } + } + + return true; +} + bool isLineInBounds(const XYZCoord& start_point, const XYZCoord& end_point) { if (doesLineIntersectPolygon(start_point, end_point, _valid_region)) { return false; @@ -145,6 +243,57 @@ bool doesLineIntersectPolygon(const XYZCoord& start_point, const XYZCoord& end_p return false; } +bool doesArcIntersectPolygon(const XYZCoord& center, double radius, double start_angle, + double sweep, const Polygon& polygon) { + for (int i = 0, j = polygon.size() - 1; i < polygon.size(); j = i++) { + if (doesArcIntersectSegment(center, radius, start_angle, sweep, polygon[i], polygon[j])) { + return true; + } + } + + return false; +} + +bool doesArcIntersectSegment(const XYZCoord& center, double radius, double start_angle, + double sweep, const XYZCoord& seg_start, const XYZCoord& seg_end) { + // parameterize the segment as P(t) = seg_start + t * d, t in [0, 1], and solve + // |P(t) - center|^2 = radius^2, a quadratic in t + // @see https://stackoverflow.com/questions/1073336/circle-line-segment-collision-detection + const XYZCoord d = seg_end - seg_start; + const XYZCoord f = seg_start - center; + + // 2D only -- z is ignored, matching the rest of the environment checks + const double a = d.x * d.x + d.y * d.y; + const double b = 2 * (f.x * d.x + f.y * d.y); + const double c = f.x * f.x + f.y * f.y - radius * radius; + + const double discriminant = b * b - 4 * a * c; + if (a == 0 || discriminant < 0) { // degenerate segment or no circle intersection + return false; + } + + const double sqrt_discriminant = std::sqrt(discriminant); + for (const double t : {(-b - sqrt_discriminant) / (2 * a), + (-b + sqrt_discriminant) / (2 * a)}) { + // hit must be within the segment + if (t < 0 || t > 1) { + continue; + } + + // hit must be within the arc's angular range: walk from start_angle in the sweep + // direction and see if the hit is reached before the sweep is used up + const double theta = + std::atan2(f.y + t * d.y, f.x + t * d.x); // angle of hit relative to center + const double travelled = (sweep > 0) ? mod(theta - start_angle, TWO_PI) + : mod(start_angle - theta, TWO_PI); + if (travelled <= std::abs(sweep)) { + return true; + } + } + + return false; +} + // Given three colinear points p, q, r, the function checks if // point q lies on line segment 'pr' bool onSegment(XYZCoord p, XYZCoord q, XYZCoord r) { @@ -357,44 +506,6 @@ std::vector findIntersectionsWithPolygon(const Polygon& polygon, return intersections; } -std::pair estimateAreaCoveredAndPathLength(const std::vector& goals) { - double area_covered = 0.0; - double path_length = 0.0; - - for (int i = 0; i < goals.size(); ++i) { - XYZCoord start_point = goals[i]; - XYZCoord end_point = goals[(i + 1) % goals.size()]; - path_length += start_point.distanceTo(end_point); - // Calculates area covered by adding the part of the line segment that is in bounds - // Caulcate the intersections and whether the start and end points are in bounds in order to - // find the sections of the line that is in bounds - if (!doesLineIntersectPolygon(start_point, end_point, _mapping_region)) { - area_covered += start_point.distanceTo(end_point) * SEARCH_RADIUS * 2; - } else { - std::vector intersections = - findIntersectionsWithPolygon(_mapping_region, start_point, end_point); - bool start_in_bounds = isPointInPolygon(_mapping_region, start_point); - bool end_in_bounds = isPointInPolygon(_mapping_region, end_point); - - if (start_in_bounds) { - area_covered += start_point.distanceTo(intersections[0]) * SEARCH_RADIUS; - intersections.erase(intersections.begin()); - } - - if (end_in_bounds) { - area_covered += end_point.distanceTo(intersections.back()) * SEARCH_RADIUS; - intersections.pop_back(); - } - - for (int j = 0; j < intersections.size(); j += 2) { - area_covered += intersections[j].distanceTo(intersections[j + 1]) * SEARCH_RADIUS; - } - } - } - - return {area_covered, path_length}; -} - Polygon scale(double scale, const Polygon& source_polygon) { Polygon scaled_polygon; diff --git a/src/pathing/rrt.cpp b/src/pathing/rrt.cpp new file mode 100644 index 00000000..2fa1261e --- /dev/null +++ b/src/pathing/rrt.cpp @@ -0,0 +1,246 @@ +#include "pathing/rrt.hpp" + +#include +#include +#include +#include +#include +#include +#include + +#include "pathing/dubins.hpp" +#include "pathing/environment.hpp" +#include "pathing/tree.hpp" +#include "utilities/constants.hpp" +#include "utilities/datatypes.hpp" +#include "utilities/logging.hpp" +#include "utilities/rng.hpp" + +std::vector> withStartAngle(std::vector> goal_angles, + double start_angle) { + if (!goal_angles.empty()) { + goal_angles[0] = {start_angle}; + } + + return goal_angles; +} + +RRT::RRT(std::vector goals, double start_angle, + std::vector> goal_angles) + : tree(RRTPoint(goals[0], start_angle)), + goals(std::move(goals)), + goal_angles(withStartAngle(std::move(goal_angles), start_angle)) {} + +RRT::RRT(std::vector goals, double start_angle, std::vector angles) + : RRT(goals, start_angle, + std::vector>(goals.size(), + angles.empty() ? DEFAULT_GOAL_ANGLES : angles)) {} + +void RRT::run() { + generateDubinsOptions(); + generateFlightPoints(); +} + +void RRT::generateDubinsOptions() { + const uint8_t total_goals = goals.size(); + + for (uint8_t cur_goal_idx = 1; cur_goal_idx < total_goals; cur_goal_idx++) { + // tries to connect directly to the goal from start + if (connectToGoal(cur_goal_idx)) { + continue; + } + + RRTIteration(cur_goal_idx); + } +} + +double RRT::pathLength() const { + double length = 0; + + for (const Leg& leg : legs) { + length += leg.length; + } + + return length; +} + +void RRT::generateFlightPoints() { + flight_path.clear(); + + for (const Leg& leg : legs) { + const std::vector points = buildFlightPath(leg); + flight_path.insert(flight_path.end(), points.begin(), points.end()); + } +} + +std::vector RRT::getPointsToGoal() const { return flight_path; } + +bool RRT::RRTIteration(uint8_t cur_goal_idx) { + std::vector sample(1); + + for (NodeId _ = 0; _ < ITERATIONS_PER_WAYPOINT; _++) { + sample[0] = RRTPoint( + Environment::getRandomPoint(false, goals[cur_goal_idx]), + random(0, TWO_PI) + ); + + // adds the sample to the tree if there is any way to fly to it + const Connection connection = bestConnection(sample); + + if (connection.isValid()) { + tree.addSample(connection.anchor, connection.end, connection.option); + } + } + + if (connectToGoal(cur_goal_idx)) { + return true; + } + + loguru::set_thread_name("Static Pathing"); + LOG_F(WARNING, "Failed to connect to goal on iteration: [%s]. Trying again...", + std::to_string(cur_goal_idx).c_str()); + + // throws away the tree that failed, keeping the same starting point + tree.setCurrentHead(tree.getStart()); + + // TODO: possiblility for infinite loop + return RRTIteration(cur_goal_idx); +} + +double RRT::lowerBound(NodeId node, const std::vector& ends) const { + const XYZCoord& anchor = tree.tree.points[node].coord; + double closest = std::numeric_limits::infinity(); + + for (const RRTPoint& end : ends) { + closest = std::min(closest, anchor.distanceTo(end.coord)); + } + + return tree.tree.length[node] + closest; +} + +void RRT::fillOptions(NodeId node, const std::vector& ends) const { + options.clear(); + const RRTPoint& anchor = tree.tree.points[node]; + const double flown = tree.tree.length[node]; + + for (const RRTPoint& end : ends) { + // gets all dubins curves from the given node to the end point + for (const RRTOption& option : Dubins::allOptions(anchor, end)) { + // filters out the options that are not valid + if (!std::isfinite(option.length)) { + continue; + } + + options.push_back({node, end, option, flown + option.length}); + } + } +} + +Connection RRT::bestConnection(const std::vector& ends) const { + for (NodeId node = 0; node < tree.tree.size; node++) { + bounds[node] = lowerBound(node, ends); + frontier[node] = node; + } + + NodeId remaining = tree.tree.size; + const auto cheapest_last = [this](NodeId a, NodeId b) { return bounds[a] > bounds[b]; }; + // sorted since after the first few nodes, the remaining often get skipped + std::make_heap(frontier.begin(), frontier.begin() + remaining, cheapest_last); + + Connection best; + while (remaining > 0) { + std::pop_heap(frontier.begin(), frontier.begin() + remaining, cheapest_last); + const NodeId node = frontier[--remaining]; + + // the rest of the tree is at least this expensive, so it cannot do better + if (bounds[node] >= best.cost) { + break; + } + + fillOptions(node, ends); + std::sort(options.begin(), options.end(), + [](const Connection& a, const Connection& b) { return a.cost < b.cost; }); + + for (const Connection& option : options) { + if (option.cost >= best.cost) { + break; + } + + if (Environment::isDubinsPathInBounds(tree.tree.points[node], + option.end, + option.option)) { + best = option; + break; + } + } + } + + return best; +} + +std::vector RRT::goalEndpoints(int cur_goal_idx) const { + std::vector ends; + ends.reserve(goal_angles[cur_goal_idx].size()); + + for (const double angle : goal_angles[cur_goal_idx]) { + ends.emplace_back(goals[cur_goal_idx], angle); + } + + return ends; +} + +bool RRT::connectToGoal(int cur_goal_idx) { + // TODO : max_paths_checked should be rearchitected + const Connection connection = bestConnection(goalEndpoints(cur_goal_idx)); + + if (!connection.isValid()) { + return false; + } + + commitConnection(connection, cur_goal_idx); + return true; +} + +void RRT::commitConnection(const Connection& connection, int cur_goal_idx) { + const NodeId goal_node = tree.tree.size; + tree.addSample(connection.anchor, connection.end, connection.option); + + legs.push_back({tree.getStart(), + tree.findPathToNode(goal_node), + connection.cost, + cur_goal_idx}); + + // the goal becomes the root of a fresh tree for the next waypoint + tree.setCurrentHead(connection.end); +} + +std::vector RRT::buildFlightPath(const Leg& leg) const { + std::vector path = Dubins::generatePath(leg.start, leg.segments); + + if (path.empty()) { + return path; + } + + // the leg is flown from the waypoint behind the one it lands on + const double start_height = goals[leg.goal_idx - 1].z; + const double height_difference = goals[leg.goal_idx].z - start_height; + + // since our points are not evenly spaced, we have to account for distance + // when doing altitude transitions. This is a misestimate + XYZCoord previous = leg.start.coord; + double total_distance = 0; + + for (XYZCoord& point : path) { + total_distance += std::hypot(point.x - previous.x, point.y - previous.y); + previous = point; + point.z = total_distance; // distance flown in path + } + + // ASSUMPTION: PATH IS NOT A BUNCH OF POINTS ON TOP OF EACH OTHER + for (XYZCoord& point : path) { + const double ratio = point.z / total_distance; + point.z = start_height + height_difference * ratio; + } + + return path; +} diff --git a/src/pathing/static.cpp b/src/pathing/static.cpp index f6d1c50f..e6fd98fd 100644 --- a/src/pathing/static.cpp +++ b/src/pathing/static.cpp @@ -4,8 +4,10 @@ #include #include +#include #include #include +#include #include #include @@ -13,6 +15,7 @@ #include "pathing/dubins.hpp" #include "pathing/environment.hpp" #include "pathing/plotting.hpp" +#include "pathing/rrt.hpp" #include "pathing/tree.hpp" #include "utilities/common.hpp" #include "utilities/constants.hpp" @@ -20,292 +23,20 @@ #include "utilities/obc_config.hpp" #include "utilities/rng.hpp" -RRT::RRT(RRTPoint start, std::vector goals, double search_radius, - const OBCConfig& config, std::vector angles) - : tree(start), - goals(goals), - iterations_per_waypoint(config.pathing.rrt.iterations_per_waypoint), - search_radius(search_radius), - rewire_radius(config.pathing.rrt.rewire_radius), - config(config.pathing.rrt) { - if (angles.size() != 0) { - this->angles = angles; - } -} - -void RRT::run() { - /* - * RRT algorithm - * - Treats each waypoint as a goal, DOES NOT reuse trees between waypoints, - * basically calls RRT for each waypoint - * - For Each Waypoint - * - Tries to connect directly to the goal - * - If it can't, it runs the RRT algorithm - * - Attempts to converge based on epoch intervals - * - If it can't, it connects to the goal with whatever it has - */ - const int total_goals = goals.size(); - - for (int current_goal_index = 0; current_goal_index < total_goals; current_goal_index++) { - // tries to connect directly to the goal - if (connectToGoal(current_goal_index)) { - continue; - } - - // run the RRT algorithm if it can not connect - RRTIteration(iterations_per_waypoint, current_goal_index); - } -} - -std::vector RRT::getPointsToGoal() const { - // return tree.getPathToGoal(); - return flight_path; -} - -bool RRT::RRTIteration(int tries, int current_goal_index) { - const int epoch_interval = tries / NUM_EPOCHS; - int current_epoch = epoch_interval; - - std::shared_ptr goal_node = nullptr; - std::shared_ptr goal_parent = nullptr; - - for (int i = 0; i < tries; i++) { - if (i == current_epoch) { - // generates a new node (not connect), and adds and breaks if it is - // within X% of the last generation - if (epochEvaluation(goal_node, goal_parent, current_goal_index)) { - return true; - } - - current_epoch += epoch_interval; - } - // generate a sample point - const RRTPoint sample = generateSamplePoint(); - - // returns all dubins options from the tree to the sample - const std::vector, RRTOption>>& options = - tree.pathingOptions(sample, config.point_fetch_method); - - // returns true if the node is successfully added to the tree - std::shared_ptr new_node = parseOptions(options, sample); - - if (new_node != nullptr && config.optimize) { - optimizeTree(new_node); - } - } - - // frees memory - // delete (goal_node); - if (!connectToGoal(current_goal_index)) { - loguru::set_thread_name("Static Pathing"); - LOG_F(WARNING, "Failed to connect to goal on iteration: [%s]. Trying again...", - std::to_string(current_goal_index).c_str()); - - if (!config.allowed_to_skip_waypoints && - !connectToGoal(current_goal_index, std::numeric_limits::max())) { - // will always return true (unless it turns into a pseudo-infinite loop) - return RRTIteration(tries, current_goal_index); - } else { - return false; - } - } - - return true; -} - -bool RRT::epochEvaluation(std::shared_ptr goal_node, std::shared_ptr goal_parent, - int current_goal_index) { - // If a single epoch has not been passed, mark this goal as the first - // benchmark. - if (goal_node == nullptr) { - goal_node = - sampleToGoal(current_goal_index, TOTAL_OPTIONS_FOR_GOAL_CONNECTION, goal_parent); - return false; - } - - std::shared_ptr new_parent = nullptr; - std::shared_ptr new_node = - sampleToGoal(current_goal_index, TOTAL_OPTIONS_FOR_GOAL_CONNECTION, new_parent); - - if (new_node == nullptr) { - return false; - } - - /* If the new node is within ~X% of the goal, then we are done. - * It should be impossible for new_node to be more inefficient than - * goal_node as it uses a superset of the tree goal_node used - */ - if (new_node->getCost() < EPOCH_TEST_MARGIN * goal_node->getCost()) { - // delete (goal_node); - goal_node = new_node; - goal_parent = new_parent; - return false; - } - - addNodeToTree(new_node, new_parent, current_goal_index); - // delete goal_node; - goal_node = nullptr; - return true; -} - -RRTPoint RRT::generateSamplePoint() const { - return RRTPoint(Environment::getRandomPoint(false, goals[0]), random(0, TWO_PI)); -} - -std::vector, RRTOption>>> -RRT::getOptionsToGoal(int current_goal_index, int total_options) const { - // attempts to connect to the goal, should always connect - std::vector goal_points; - - // Generates goal specific points based on current Waypoints and list og - // Angles - for (const double angle : angles) { - goal_points.push_back(RRTPoint(goals[current_goal_index], angle)); - } - - // RRTPoint is the goal that is to be connected - // RRTNode is the node in the tree that is the anchor - // RRTOPtion Node-->Point - std::vector, RRTOption>>> all_options; - - // limit amount of options to sort, defined in constants.hpp - const int NUMBER_OPTIONS_EACH = total_options / angles.size(); - - // gets all options for each of the goals, and puts them into a unified list - // TODO ? maybe better for a max heap? - for (const RRTPoint& goal : goal_points) { - const std::vector, RRTOption>>& options = - // For now, we use optimal pathing - tree.pathingOptions(goal, PointFetchMethod::Enum::NONE, NUMBER_OPTIONS_EACH); - - for (const auto& [node, option] : options) { - all_options.push_back({goal, {node, option}}); - } - } - - std::sort(all_options.begin(), all_options.end(), [](const auto& a, const auto& b) { - auto& [a_goal, a_paths] = a; - auto& [a_node, a_option] = a_paths; - auto& [b_goal, b_paths] = b; - auto& [b_node, b_option] = b_paths; - return a_option.length + a_node->getCost() < b_option.length + b_node->getCost(); - }); - - return all_options; -} - -std::shared_ptr RRT::sampleToGoal(int current_goal_index, int total_options, - std::shared_ptr& parent) const { - // gets all options for each of the goals - const auto& all_options = getOptionsToGoal(current_goal_index, total_options); - - // - for (const auto& [goal, pair] : all_options) { - auto& [anchor_node, option] = pair; - - std::shared_ptr new_node = tree.generateNode(anchor_node, goal, option); - - if (new_node != nullptr) { - parent = anchor_node; - return new_node; - } - } - - return nullptr; -} - -bool RRT::connectToGoal(int current_goal_index, int total_options) { - std::shared_ptr parent = nullptr; - std::shared_ptr goal_node = sampleToGoal(current_goal_index, total_options, parent); - - if (goal_node == nullptr) { - return false; - } - - addNodeToTree(goal_node, parent, current_goal_index); - return true; -} - -void RRT::addNodeToTree(std::shared_ptr goal_node, std::shared_ptr parent, - int current_goal_index) { - // add the node to the tree - tree.addNode(parent, goal_node); - - // inserts the altitude into the path - std::vector local_path = tree.getPathSegment(goal_node); - - double start_height; - if (current_goal_index == 0) { - start_height = tree.getStart().coord.z; - } else { - start_height = goals[current_goal_index - 1].z; - } - - double height_difference = goals[current_goal_index].z - start_height; - double height_increment = height_difference / local_path.size(); - - for (XYZCoord& point : local_path) { - point.z = start_height; - start_height += height_increment; - } - - // adds local path to the flight path, and updates the tree - flight_path.insert(flight_path.end(), local_path.begin(), local_path.end()); - tree.setCurrentHead(goal_node); -} - -std::shared_ptr RRT::parseOptions( - const std::vector, RRTOption>>& options, - const RRTPoint& sample) { - for (auto& [node, option] : options) { - /* - * stop if - * 1. the node is null - * 2. the node is the same as the sample - * - * The idea is that any further options will have the same if not more - * issues - * - * This shouldn't ever happen? - */ - // if (node == nullptr || node->getPoint() == sample) { - // return nullptr; - - // else, attempt to add the node to the tree - std::shared_ptr sucessful_addition = tree.addSample(node, sample, option); - - if (sucessful_addition != nullptr) { - return sucessful_addition; - } - } - - return nullptr; -} - -void RRT::optimizeTree(std::shared_ptr sample) { tree.RRTStar(sample, rewire_radius); } - ForwardCoveragePathing::ForwardCoveragePathing(const RRTPoint& start, double scan_radius, const OBCConfig& config) : scan_radius(scan_radius), start(start), config(config.pathing.coverage) {} std::vector ForwardCoveragePathing::run() const { - return coverageDefault(); - // return config.forward.optimize ? coverageOptimal() : coverageDefault(); + // return coverageDefault(); + return config.forward.optimize ? coverageOptimal() : coverageDefault(); } std::vector ForwardCoveragePathing::coverageDefault() const { - // generates the endpoints for the lines (including headings) - std::vector waypoints = Environment::getAirdropWaypoints( - scan_radius, config.forward.one_way, config.forward.vertical); - waypoints.emplace(waypoints.begin(), start); - - // 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])); - } + RRT rrt = pathScanLines(config.forward.one_way, config.forward.vertical); + rrt.generateFlightPoints(); - return generatePath(dubins_options, waypoints); + return rrt.getPointsToGoal(); } std::vector ForwardCoveragePathing::coverageOptimal() const { @@ -317,84 +48,71 @@ std::vector ForwardCoveragePathing::coverageOptimal() const { * [3] - one_way, horizontal */ - std::vector> configs = { + const std::vector> layouts = { {false, true}, {false, false}, {true, true}, {true, false}}; - std::vector> dubins_paths; - std::vector lengths = {0, 0, 0, 0}; - - // generates the endpoints for the lines (including headings) - for (int i = 0; i < configs.size(); i++) { - const auto& config = configs[i]; - - std::vector waypoints = - Environment::getAirdropWaypoints(scan_radius, config.first, config.second); + /* + * Which layout is cheapest cannot be told from the scan lines alone -- what a + * layout costs is the flying it takes to get from one line to the next and + * around whatever is in the way, which is not known until it has been pathed. + * So all four are pathed, and only the one that wins is ever flown. + */ + std::optional best; - // generates the path connecting the waypoints to each other - std::vector current_dubins_path; + for (const std::pair& layout : layouts) { + RRT rrt = pathScanLines(layout.first, layout.second); - for (int i = 0; i < waypoints.size() - 1; i++) { - RRTOption dubins_path = Dubins::bestOption(waypoints[i], waypoints[i + 1]); - lengths[i] += dubins_path.length; - current_dubins_path.push_back(dubins_path); + if (!best.has_value() || rrt.pathLength() < best->pathLength()) { + best.emplace(std::move(rrt)); } - - dubins_paths.push_back(current_dubins_path); } - // finds the shortest path - int best_path_idx = 0; - double shortest_length = lengths[0]; - for (int i = 1; i < lengths.size(); i++) { - if (lengths[i] < shortest_length) { - shortest_length = lengths[i]; - best_path_idx = i; - } + if (!best.has_value()) { + return {}; } - // gets the path - std::vector waypoints = Environment::getAirdropWaypoints( - scan_radius, configs[best_path_idx].first, configs[best_path_idx].second); - - waypoints.emplace(waypoints.begin(), start); - - return generatePath(dubins_paths[best_path_idx], waypoints); + best->generateFlightPoints(); + return best->getPointsToGoal(); } -std::vector ForwardCoveragePathing::generatePath( - const std::vector& dubins_options, const std::vector& waypoints) const { - std::vector path; +std::vector ForwardCoveragePathing::scanLines(bool one_way, bool vertical) const { + std::vector waypoints = + Environment::getAirdropWaypoints(scan_radius, one_way, vertical); - // height adjustement - double height = waypoints[0].coord.z; - double height_difference = config.altitude_m - waypoints[0].coord.z; - - std::vector path_coordinates = Dubins::generatePoints( - waypoints[0], waypoints[1], dubins_options[0].dubins_path, dubins_options[0].has_straight); + // the whole sweep is flown at one altitude, so only the way in is a climb + for (RRTPoint& waypoint : waypoints) { + waypoint.coord.z = config.altitude_m; + } - double height_increment = height_difference / path_coordinates.size(); + // the plane flies from where it is now, so that is the first of the waypoints + waypoints.insert(waypoints.begin(), start); - for (XYZCoord& coord : path_coordinates) { - coord.z = height; - height += height_increment; - } + return waypoints; +} - path.insert(path.end(), path_coordinates.begin() + 1, path_coordinates.end()); +RRT ForwardCoveragePathing::pathScanLines(bool one_way, bool vertical) const { + const std::vector waypoints = scanLines(one_way, vertical); - // 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_options[i].has_straight); + std::vector goals; + goals.reserve(waypoints.size()); - for (XYZCoord& coord : path_coordinates) { - coord.z = config.altitude_m; - } + /* + * A scan line only covers the ground it is meant to if it is flown along its + * own direction, so each waypoint is left exactly one way to be reached and + * RRT is only free to choose the flying between them. + */ + std::vector> goal_angles; + goal_angles.reserve(waypoints.size()); - path.insert(path.end(), path_coordinates.begin() + 1, path_coordinates.end()); + for (const RRTPoint& waypoint : waypoints) { + goals.push_back(waypoint.coord); + goal_angles.push_back({waypoint.psi}); } - return path; + RRT rrt(std::move(goals), start.psi, std::move(goal_angles)); + rrt.generateDubinsOptions(); + + return rrt; } HoverCoveragePathing::HoverCoveragePathing(std::shared_ptr state) @@ -491,7 +209,12 @@ AirdropApproachPathing::AirdropApproachPathing(const RRTPoint& start, const XYZC std::vector AirdropApproachPathing::run() const { RRTPoint drop_vector = getDropLocation(); - RRT rrt(start, {drop_vector.coord}, SEARCH_RADIUS, config, {drop_vector.psi}); + + // the drop is only a drop if it is flown at the heading that lines the plane + // up with the target, so that is the one way the goal may be reached + const std::vector approach_angles = {drop_vector.psi}; + + RRT rrt({start.coord, drop_vector.coord}, start.psi, approach_angles); rrt.run(); return rrt.getPointsToGoal(); @@ -522,59 +245,6 @@ RRTPoint AirdropApproachPathing::getDropLocation() const { return RRTPoint(drop_location, angle); } -std::vector> generateGoalListDeviations(const std::vector& goals, - XYZCoord deviation_point) { - std::vector> goal_list_deviations; - for (int i = 0; i < goals.size() + 1; i++) { - std::vector goal_list_deviation = goals; - goal_list_deviation.insert(goal_list_deviation.begin() + i, deviation_point); - goal_list_deviations.push_back(goal_list_deviation); - } - - return goal_list_deviations; -} - -std::vector> generateRankedNewGoalsList(const std::vector& goals) { - // generate deviation points randomly in the mapping region - std::vector deviation_points; - for (int i = 0; i < 200; i++) { - deviation_points.push_back(Environment::getRandomPoint(true, goals[0])); - } - - // each deviation point can be inserted between any two goals - std::vector> new_goals_list; - for (const XYZCoord& deviation_point : deviation_points) { - std::vector> goal_list_deviations = - generateGoalListDeviations(goals, deviation_point); - new_goals_list.insert(new_goals_list.end(), goal_list_deviations.begin(), - goal_list_deviations.end()); - } - - // run each goal list and get the area covered and the length of the path - std::vector> area_length_pairs; - for (const std::vector& new_goals : new_goals_list) { - area_length_pairs.push_back(Environment::estimateAreaCoveredAndPathLength(new_goals)); - } - - // rank the new goal lists by the area covered and the length of the path - std::vector>> ranked_new_goals_list; - for (int i = 0; i < new_goals_list.size(); i++) { - ranked_new_goals_list.push_back( - {area_length_pairs[i].first / area_length_pairs[i].second, new_goals_list[i]}); - } - - std::sort(ranked_new_goals_list.begin(), ranked_new_goals_list.end(), - [](const auto& a, const auto& b) { return a.first > b.first; }); - - // return the ranked list of new goals lists - std::vector> ranked_goals; - for (const auto& pair : ranked_new_goals_list) { - ranked_goals.push_back(pair.second); - } - - return ranked_goals; -} - RRTPoint getCurrentLoc(std::shared_ptr state) { std::shared_ptr mav = state->getMav(); std::pair start_lat_long = mav->latlng_deg(); @@ -616,15 +286,13 @@ std::vector generateInitialPath(std::shared_ptr state) { std::vector goals = state->mission_params.getWaypoints(); - // update goals here - if (state->config.pathing.rrt.generate_deviations) { - goals = generateRankedNewGoalsList(goals)[0]; - } - RRTPoint start = getCurrentLoc(state); start.coord.z = state->config.takeoff.altitude_m; - RRT rrt(start, goals, SEARCH_RADIUS, state->config); + // the plane flies from where it is now, so that is the first of the waypoints + goals.insert(goals.begin(), start.coord); + + RRT rrt(goals, start.psi); rrt.run(); @@ -649,10 +317,6 @@ std::vector generateNextWaypointPath(std::shared_ptr sta std::vector goals = state->mission_params.getWaypoints(); - if (state->config.pathing.rrt.generate_deviations) { - goals = generateRankedNewGoalsList(goals)[0]; - } - RRTPoint start(goals.back(), start_angle); // add buffer to the start point so that we dont loopty loop @@ -662,7 +326,10 @@ std::vector generateNextWaypointPath(std::shared_ptr sta start.coord.y += buffer_m * std::sin(start_angle); } - RRT rrt(start, goals, SEARCH_RADIUS, state->config); + // the plane flies from where it is now, so that is the first of the waypoints + goals.insert(goals.begin(), start.coord); + + RRT rrt(goals, start.psi); rrt.run(); @@ -685,6 +352,7 @@ std::vector generateSearchPath(std::shared_ptr state, do "Waypoint path is empty. Failed to generate search path"); return {}; } + RRTPoint start(state->mission_params.getWaypoints().back(), start_angle); double scan_radius = state->config.pathing.coverage.camera_vision_m; diff --git a/src/pathing/tree.cpp b/src/pathing/tree.cpp index ad3135db..3f226f95 100644 --- a/src/pathing/tree.cpp +++ b/src/pathing/tree.cpp @@ -1,489 +1,58 @@ #include "pathing/tree.hpp" #include -#include -#include -#include -#include -#include -#include #include #include "pathing/dubins.hpp" -#include "pathing/environment.hpp" #include "utilities/datatypes.hpp" -#include "utilities/logging.hpp" -#include "utilities/rng.hpp" -#include "utilities/obc_config.hpp" -RRTNode::RRTNode(const RRTPoint& point, double cost, double path_length, - const std::vector path) - : point{point}, cost{cost}, path_length(path_length), path(path) {} +RRTTree::RRTTree(RRTPoint root_point) { this->setCurrentHead(root_point); } -RRTNode::RRTNode(const RRTPoint& point, double cost, double path_length, - const std::vector path, RRTNodeList reachable) - : point{point}, cost{cost}, path_length(path_length), path(path), reachable{reachable} {} +void RRTTree::addSample(NodeId parent, const RRTPoint new_point, const RRTOption option) { + NodeId prev_sibling = tree.first_child[parent]; + NodeId idx = tree.alloc(); -bool RRTNode::operator==(const RRTNode& other_node) const { - return this->point == other_node.point && this->cost == other_node.cost; -} - -RRTPoint& RRTNode::getPoint() { return this->point; } - -void RRTNode::setReachable(const RRTNodeList& reachable) { - this->reachable = reachable; -} - -void RRTNode::addReachable(std::shared_ptr new_node) { - this->reachable.push_back(new_node); -} - -void RRTNode::removeReachable(std::shared_ptr old_node) { - for (int i = 0; i < reachable.size(); i++) { - if (reachable.at(i) == old_node) { - reachable.erase(reachable.begin() + i); - return; - } - } -} - -const RRTNodeList& RRTNode::getReachable() { return (this->reachable); } - -double RRTNode::getCost() const { return this->cost; } - -void RRTNode::setCost(double new_cost) { this->cost = new_cost; } - -const std::vector& RRTNode::getPath() const { return this->path; } - -void RRTNode::setPath(const std::vector& path) { this->path = path; } - -double RRTNode::getPathLength() const { return this->path_length; } - -void RRTNode::setPathLength(double new_path_length) { this->path_length = new_path_length; } -/* - - - - -*/ -/** RRTTree */ -/* - - - - -*/ - -RRTTree::RRTTree(RRTPoint root_point) : tree_size(1) { - std::shared_ptr new_node = - std::make_shared(root_point, 0, 0, std::vector{}); - root = new_node; - current_head = new_node; -} - -// TODO - seems a bit sketchy -RRTTree::~RRTTree() { } - -bool RRTTree::validatePath(const std::vector& path, const RRTOption& option) const { - return Environment::isPathInBounds(path); -} - -std::shared_ptr RRTTree::generateNode(std::shared_ptr anchor_node, - const RRTPoint& new_point, - const RRTOption& option) const { - const std::vector& path = Dubins::generatePoints( - anchor_node->getPoint(), new_point, option.dubins_path, option.has_straight); - - if (!validatePath(path, option)) { - return nullptr; - } - - // needs to add the node to the tree - std::shared_ptr new_node = - std::make_shared(new_point, - anchor_node->getCost() + option.length, - option.length, - path); - - return new_node; -} - -bool RRTTree::addNode(std::shared_ptr anchor_node, std::shared_ptr new_node) { - if (new_node == nullptr || anchor_node == nullptr) { - return false; - } - - anchor_node->addReachable(new_node); - tree_size++; - return true; -} - -// TODO - convert from old to new -std::shared_ptr RRTTree::addSample(std::shared_ptr anchor_node, - const RRTPoint& new_point, - const RRTOption& option) { - std::shared_ptr new_node = generateNode(anchor_node, new_point, option); - - if (addNode(anchor_node, new_node)) { - return new_node; - } - - return nullptr; -} - -void RRTTree::rewireEdge(std::shared_ptr current_node, - std::shared_ptr previous_parent, - std::shared_ptr new_parent, - const std::vector& path, - double path_cost) { - // ORDER MATTERS, REMOVE THEN ADD TO PRESERVE THE CURR_NODE HAS A PARENT - previous_parent->removeReachable(current_node); - new_parent->addReachable(current_node); - - // bubbles down the tree to reassign the costs - current_node->setPath(path); - current_node->setCost(new_parent->getCost() + path_cost); - current_node->setPathLength(path_cost); - reassignCosts(current_node); -} - -std::vector> RRTTree::getKRandomNodes(int k) const { - std::vector> nodes; - // proabability that any given node should be added - double chance = 1.0 * k / tree_size; - getKRandomNodesRecursive(nodes, current_head, chance); - - return nodes; -} - -void RRTTree::getKRandomNodesRecursive(std::vector>& nodes, - std::shared_ptr current_node, - double chance) const { - if (current_node == nullptr) { - return; - } - - // if the chance is less than the random number, then add the node to the list - // TODO maybe make some check that prevents the random calls if the tree is small enough - if (random(0, 1) < chance) { - nodes.emplace_back(current_node); - } - - for (std::shared_ptr node : current_node->getReachable()) { - getKRandomNodesRecursive(nodes, node, chance); - } -} - -std::vector> RRTTree::getKClosestNodes(const RRTPoint& sample, - int k) const { - std::vector> closest_nodes; - - // helper vector that associates nodes with distances - // TODO - do some benchmarks with max-heaps to see which one is more efficient - std::vector>> nodes_by_distance; - getKClosestNodesRecursive(nodes_by_distance, sample, current_head); - - // sorts the nodes by distance - std::sort(nodes_by_distance.begin(), nodes_by_distance.end(), - [](auto& left, auto& right) { return left.first < right.first; }); - - // gets either the k closest nodes, or the entire list - int size = nodes_by_distance.size(); - int stop_condition = std::min(k, size); - for (int i = 0; i < stop_condition; i++) { - closest_nodes.emplace_back(nodes_by_distance[i].second); - } - - return closest_nodes; -} - -void RRTTree::getKClosestNodesRecursive( - std::vector>>& nodes_by_distance, - const RRTPoint& sample, - std::shared_ptr current_node) const { - if (current_node == nullptr) { - return; - } - - // ONLY considers the distance, and not the path length required to get to the node - double distance = sample.coord.distanceToSquared(current_node->getPoint().coord); - nodes_by_distance.push_back({distance, current_node}); - - for (std::shared_ptr node : current_node->getReachable()) { - getKClosestNodesRecursive(nodes_by_distance, sample, node); - } -} - -void RRTTree::fillOptionsNodes(std::vector, RRTOption>>& options, - const std::vector>& nodes, - const RRTPoint& sample) const { - for (std::shared_ptr node : nodes) { - const std::vector& local_options = Dubins::allOptions(node->getPoint(), sample); + tree.points[idx] = new_point; + tree.rrt_options[idx] = option; + tree.parent[idx] = parent; + tree.length[idx] = tree.length[parent] + option.length; + tree.first_child[idx] = INVALID_NODE; + tree.next_sibling[idx] = INVALID_NODE; - for (const RRTOption& option : local_options) { - if (std::isnan(option.length) || - option.length == std::numeric_limits::infinity()) { - continue; - } - - options.push_back({node, option}); - } - } -} - -std::shared_ptr RRTTree::getRoot() const { return this->root; } - -/* - TODO - investigate whether a max heap is better or worse -*/ -std::vector, RRTOption>> RRTTree::pathingOptions( - const RRTPoint& end, PointFetchMethod::Enum point_fetch_method, int quantity_options) const { - // fills the options list with valid values - std::vector, RRTOption>> options; - - switch (point_fetch_method) { - case PointFetchMethod::Enum::RANDOM: { - const RRTNodeList& nodes = getKRandomNodes(K_RANDOM_NODES); - fillOptionsNodes(options, nodes, end); - } break; - case PointFetchMethod::Enum::NEAREST: { - const RRTNodeList& nodes = getKClosestNodes(end, K_CLOESEST_NODES); - fillOptionsNodes(options, nodes, end); - } break; - case PointFetchMethod::Enum::NONE: - fillOptions(options, current_head, end); - break; - default: - fillOptions(options, current_head, end); - break; - } - - // sorts the list - std::sort(options.begin(), options.end(), [](auto& a, auto& b) { - auto& [a_node, a_option] = a; - auto& [b_node, b_option] = b; - return a_option.length + a_node->getCost() < b_option.length + b_node->getCost(); - }); - - // the options are already sorted, why return a truncated list? - // 2024-03-11 : because pathing to goal req a certain amount, you can change this later if you - // want, idk c++ memory management well enough to know if tht is a good idea (for speed) - - // if there are less options than needed amount, then just reurn the xisting list, else, - // return a truncated list. - if (options.size() < quantity_options) { - return options; - } - - // erases everything after the last wanted element - options.erase(options.begin() + quantity_options, options.end()); - - return options; -} - -void RRTTree::fillOptions(std::vector, RRTOption>>& options, - std::shared_ptr node, - const RRTPoint& end) const { - /* - TODO - try to limit the scope of the search to prevent too many calls to dubins - */ - if (node == nullptr) { - return; - } - - // gets all dubins curves from the current node to the end point - const std::vector& local_options = Dubins::allOptions(node->getPoint(), end); - - // filters out the options that are not valid - for (const RRTOption& option : local_options) { - if (std::isnan(option.length) || option.length == std::numeric_limits::infinity()) { - continue; - } - - options.push_back({node, option}); - } - - // recursively calls the function for all reachable nodes - for (std::shared_ptr child : node->getReachable()) { - fillOptions(options, child, end); - } -} - -void RRTTree::RRTStar(std::shared_ptr sample, double rewire_radius) { - // last element takes in the squared value of rewire_radius to prevent the need for sqrt() - RRTStarRecursive(current_head, sample, rewire_radius * rewire_radius); -} - -void RRTTree::setCurrentHead(std::shared_ptr goal) { - if (goal == nullptr) { - LOG_F(ERROR, "FAILURE: Goal is not in the tree"); - return; - } - - // update local parametters - tree_size = 1; - current_head = goal; -} - -std::vector RRTTree::getPathSegment(std::shared_ptr node) const { - if (node == current_head) { - return {}; - } - - std::vector> nodes = findPathToNode(node); - return buildPathFromNodes(nodes); -} - -RRTNodeList RRTTree::findPathToNode(std::shared_ptr target_node) const { - if (target_node == current_head) { - LOG_F(WARNING, "TREE: target_node and current_head are the same"); - return {target_node, target_node}; - } - - // Stack stores pairs of (node, next_child_index) - std::stack, size_t>> dfs_stack; - std::vector> current_path; - - dfs_stack.push({current_head, 0}); - current_path.push_back(current_head); - - while (!dfs_stack.empty()) { - std::shared_ptr current_node = dfs_stack.top().first; - size_t& child_idx = dfs_stack.top().second; - - if (current_node == target_node) { - if (current_path.size() <= 1) { - LOG_F(WARNING, "TREE: target_node and current_head are the same"); - return {target_node, target_node}; - } - - // Return path excluding current_head (index 0) - return RRTNodeList(current_path.begin() + 1, current_path.end()); - } - - const RRTNodeList& children = current_node->getReachable(); - - if (child_idx < children.size()) { - std::shared_ptr next_child = children[child_idx]; - child_idx++; - - dfs_stack.push({next_child, 0}); - current_path.push_back(next_child); - } else { - // All children visited, backtrack - dfs_stack.pop(); - current_path.pop_back(); + if (prev_sibling == INVALID_NODE) { + tree.first_child[parent] = idx; + } else { + while (tree.next_sibling[prev_sibling] != INVALID_NODE) { + prev_sibling = tree.next_sibling[prev_sibling]; } + tree.next_sibling[prev_sibling] = idx; } - - LOG_F(ERROR, "TREE: Iterative DFS failed to find target_node from current_head"); - return {}; } -std::vector RRTTree::buildPathFromNodes(const RRTNodeList& nodes) const { - std::vector path; - for (const auto& node : nodes) { - const std::vector& edge_path = node->getPath(); - // Skip the first point to avoid duplicates (it's the end point of previous edge) - if (!edge_path.empty()) { - path.insert(path.end(), edge_path.begin() + 1, edge_path.end()); - } - } - return path; -} - -RRTPoint& RRTTree::getStart() const { return root->getPoint(); } - -/*-----------------*/ -/* RRTTree Private */ -/*-----------------*/ - -void RRTTree::RRTStarRecursive(std::shared_ptr current_node, - std::shared_ptr sample, - double rewire_radius_squared) { - // base case - if (current_node == nullptr) { - return; - } - - // for all nodes past the current node, attempt to rewire them - // Use a copy of reachable nodes because rewireEdge modifies the list - RRTNodeList children = current_node->getReachable(); - for (std::shared_ptr child : children) { - // get the distance between the current node and the nearest node - if (child->getPoint().distanceToSquared(sample->getPoint()) > rewire_radius_squared) { - continue; - } - - // the sample shouldn't have any children - if (child == sample) { - return; - } - - // get the dubins options (sorted) - std::vector options = - Dubins::allOptions(sample->getPoint(), child->getPoint()); - std::sort(options.begin(), options.end(), compareRRTOptionLength); - - - // for each option - for (const RRTOption& option : options) { - if (std::isnan(option.length) || - option.length == std::numeric_limits::infinity()) { - break; - } - - // if the node is uncompetitive, move onto the next node - double new_cost = sample->getCost() + option.length; - double cost = child->getCost(); - - if (new_cost >= cost) { - break; - } +void RRTTree::setCurrentHead(RRTPoint goal) { + tree.reset(); - // 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); + NodeId idx = tree.alloc(); - if (!validatePath(path, option)) { - continue; - } - - // rewire the edge - rewireEdge(child, current_node, sample, path, option.length); - } - } - - // recurse - for (std::shared_ptr child : current_node->getReachable()) { - RRTStarRecursive(child, sample, rewire_radius_squared); - } + tree.points[idx] = goal; + tree.rrt_options[idx] = RRTOption(0.0, DubinsPath(0.0, 0.0, 0.0), true); + tree.parent[idx] = INVALID_NODE; + tree.length[idx] = 0.0; + tree.first_child[idx] = INVALID_NODE; + tree.next_sibling[idx] = INVALID_NODE; } -void RRTTree::reassignCosts(std::shared_ptr changed_node) { - if (changed_node == nullptr) { - return; - } +RRTPoint RRTTree::getStart() const { return tree.points[0]; } - for (std::shared_ptr child : changed_node->getReachable()) { - reassignCostsRecursive(changed_node, child, changed_node->getCost()); - } -} +std::vector RRTTree::findPathToNode(NodeId target_node) const { + std::vector segments; -void RRTTree::reassignCostsRecursive(std::shared_ptr parent, - std::shared_ptr current_node, - double path_cost) { - if (current_node == nullptr) { - return; + for (NodeId node = target_node; + node != 0 && node != INVALID_NODE; + node = tree.parent[node]) { + segments.emplace_back(tree.points[node], tree.rrt_options[node]); } - // reassigns the cost: cost to get to the parent + known path length between parent and child - current_node->setCost(path_cost + current_node->getPathLength()); - for (std::shared_ptr neighbor : current_node->getReachable()) { - reassignCostsRecursive(current_node, neighbor, current_node->getCost()); - } + std::reverse(segments.begin(), segments.end()); + return segments; } diff --git a/src/utilities/obc_config.cpp b/src/utilities/obc_config.cpp index f4b4c3f8..0fdf3ea3 100644 --- a/src/utilities/obc_config.cpp +++ b/src/utilities/obc_config.cpp @@ -60,15 +60,7 @@ OBCConfig::OBCConfig(int argc, char* argv[]) { SET_CONFIG_OPT(network, gcs, port); SET_CONFIG_OPT(pathing, laps); - SET_CONFIG_OPT(pathing, rrt, iterations_per_waypoint); SET_CONFIG_OPT(pathing, upload_distance_buffer_m); - SET_CONFIG_OPT(pathing, rrt, rewire_radius); - SET_CONFIG_OPT(pathing, rrt, optimize); - SET_CONFIG_OPT(pathing, rrt, generate_deviations); - - SET_CONFIG_OPT_VARIANT(PointFetchMethod, pathing, rrt, point_fetch_method); - - SET_CONFIG_OPT(pathing, rrt, allowed_to_skip_waypoints); SET_CONFIG_OPT_VARIANT(AirdropCoverageMethod, pathing, coverage, method); diff --git a/tests/integration/CMakeLists.txt b/tests/integration/CMakeLists.txt index 091dc909..bad241a0 100644 --- a/tests/integration/CMakeLists.txt +++ b/tests/integration/CMakeLists.txt @@ -115,16 +115,6 @@ target_add_mavsdk(airdrop_approach) target_add_matplot(airdrop_approach) target_add_loguru(airdrop_approach) -add_executable(deviation_ranking "deviation_ranking.cpp") -target_link_libraries(deviation_ranking PRIVATE obcpp_lib) -target_include_directories(deviation_ranking PRIVATE ${INCLUDE_DIRECTORY}) -target_add_json(deviation_ranking) -target_add_httplib(deviation_ranking) -target_add_mavsdk(deviation_ranking) -target_add_matplot(deviation_ranking) -target_add_loguru(deviation_ranking) - - add_executable(airdrop_packets "airdrop_packets.cpp") target_link_libraries(airdrop_packets PRIVATE obcpp_lib) target_include_directories(airdrop_packets PRIVATE ${INCLUDE_DIRECTORY}) diff --git a/tests/integration/deviation_ranking.cpp b/tests/integration/deviation_ranking.cpp deleted file mode 100644 index 8bde7138..00000000 --- a/tests/integration/deviation_ranking.cpp +++ /dev/null @@ -1,105 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include - -#include "core/mission_state.hpp" -#include "handler_params.hpp" -#include "network/gcs.hpp" -#include "network/gcs_macros.hpp" -#include "network/gcs_routes.hpp" -#include "pathing/plotting.hpp" -#include "pathing/static.hpp" -#include "ticks/mav_upload.hpp" -#include "ticks/mission_prep.hpp" -#include "ticks/path_gen.hpp" -#include "ticks/tick.hpp" -#include "utilities/constants.hpp" -#include "utilities/datatypes.hpp" -#include "utilities/http.hpp" - -/* - * FILE OUTPUT LOCATIONS - * |-- build - * |-- pathing_output - * |-- deviation.jpg - * |-- deviation_path.gif (if enabled) - * |-- deviation_coordinates.txt - * - * This rough integration test is to test the airdrop search pathing algorithm - */ -int main() { - std::ofstream file; - - LOG_F(WARNING, "Deviation Ranking Testing"); - - // Read mission data from JSON file - std::ifstream mission_file("../tests/integration/util/mission_data_2020.json"); - if (!mission_file.is_open()) { - LOG_F(ERROR, "Failed to open mission_data_2020.json"); - return 1; - } - - std::stringstream buffer; - buffer << mission_file.rdbuf(); - std::string mission_json_2020 = buffer.str(); - mission_file.close(); - - // Upload the Mission - DECLARE_HANDLER_PARAMS(state, req, resp); - req.body = mission_json_2020; - state->setTick(new MissionPrepTick(state)); - - // create the environment with custome mapping region - Polygon mapping_region = {XYZCoord(-200, 150, 0), XYZCoord(100, 75, 0), XYZCoord(-125, 300, 0), - XYZCoord(-300, 300, 0)}; - Environment::init(state->mission_params.getFlightBoundary(), - state->mission_params.getAirdropBoundary(), mapping_region); - std::vector goals = state->mission_params.getWaypoints(); - - auto start_time = std::chrono::high_resolution_clock::now(); - std::vector> rank_new_goals_list = generateRankedNewGoalsList(goals); - auto end_time = std::chrono::high_resolution_clock::now(); - std::chrono::duration elapsed = end_time - start_time; - LOG_F(INFO, "Time to run: %f s", elapsed.count()); - - std::vector new_goals = rank_new_goals_list[0]; - RRTPoint start = RRTPoint(new_goals[0], 0); - start.coord.z = 100; - new_goals.erase(new_goals.begin()); - - double search_radius = 9999; - - RRT rrt = RRT(start, new_goals, search_radius, state->config); - - // run the rrt algorithm - rrt.run(); - - // get the path, put it into the file - std::vector path = rrt.getPointsToGoal(); - LOG_F(INFO, "Path size: %zu", path.size()); - LOG_F(INFO, "Path length: %f", path.size() * state->config.pathing.dubins.point_separation); - - // files to put path_coordinates to - file.open("pathing_output/deviation_coordinates.txt"); - for (const XYZCoord& point : path) { - std::ostringstream oss; - oss << std::fixed << std::setprecision(6) - << std::setw(12) << point.x << ", " - << std::setw(12) << point.y << ", " - << std::setw(12) << point.z << '\n'; - file << oss.str(); - } - file.close(); - - // plot the path - PathingPlot plotter("pathing_output", state->mission_params.getFlightBoundary(), - mapping_region, new_goals); - // plotter.addFinalPolyline(path); - plotter.output("deviation_path", PathOutputType::STATIC); - - return 0; -} \ No newline at end of file diff --git a/tests/integration/path_planning.cpp b/tests/integration/path_planning.cpp index 957cf7f7..386fe57d 100644 --- a/tests/integration/path_planning.cpp +++ b/tests/integration/path_planning.cpp @@ -14,6 +14,7 @@ #include "network/gcs_macros.hpp" #include "network/gcs_routes.hpp" #include "pathing/plotting.hpp" +#include "pathing/rrt.hpp" #include "pathing/static.hpp" #include "ticks/mission_prep.hpp" #include "ticks/mav_upload.hpp" @@ -73,7 +74,8 @@ int main() { goals.push_back(waypoint); } - goals.erase(goals.begin()); + // the plane starts out on the first waypoint, 30 meters up after takeoff + goals[0].z = 30.0; Polygon obs1 = {XYZCoord(-200, 150, 0), XYZCoord(100, 75, 0), XYZCoord(-125, 300, 0), XYZCoord(-300, 300, 0)}; @@ -83,19 +85,13 @@ int main() { std::vector obstacles = {obs1, obs2}; - RRTPoint start = RRTPoint(state->mission_params.getWaypoints()[0], 0); - start.coord.z = 30.0; // 30 meters takeoff - - // RRT settings (manually put in) - double search_radius = 9999; LOG_F(WARNING, "RRT Stats"); - LOG_F(INFO, "Search Radius %f", search_radius); Environment::init(state->mission_params.getFlightBoundary(), state->mission_params.getAirdropBoundary(), state->mission_params.getAirdropBoundary(), obstacles); - RRT rrt = RRT(start, goals, search_radius, state->config); + RRT rrt = RRT(goals, 0); //run the algoritm, and time it auto start_time = std::chrono::high_resolution_clock::now(); diff --git a/tests/unit/environment_test.cpp b/tests/unit/environment_test.cpp index 1a32dc81..88313096 100644 --- a/tests/unit/environment_test.cpp +++ b/tests/unit/environment_test.cpp @@ -2,6 +2,11 @@ #include +#include +#include +#include + +#include "pathing/dubins.hpp" #include "utilities/constants.hpp" #include "utilities/datatypes.hpp" @@ -298,4 +303,837 @@ TEST(EnvironmentTest, FindIntersectionsWithPolygon) { std::vector result5 = Environment::findIntersectionsWithPolygon(mapping_region2, start5, end5); EXPECT_EQ(expect5, result5); -} \ No newline at end of file +} +/* + * ============================================================================ + * Environment::isDubinsPathInBounds and its arc helpers + * + * The analytic checks are allowed to be CONSERVATIVE (reject a path that is + * actually clear, e.g. one that grazes a boundary), but must never be + * PERMISSIVE (accept a path that leaves the region or clips an obstacle). + * The differential tests below assert that direction specifically. + * ============================================================================ + */ + +namespace { + +// 100 x 100 field, nothing in it +void initOpenField() { + Polygon field = {{XYZCoord(0, 0, 0), XYZCoord(100, 0, 0), XYZCoord(100, 100, 0), + XYZCoord(0, 100, 0)}}; + Environment::init(field, {}, {}, {}); +} + +// 100 x 100 field with a 20 x 20 obstacle dead center, spanning [40, 60] on both axes +void initFieldWithObstacle() { + Polygon field = {{XYZCoord(0, 0, 0), XYZCoord(100, 0, 0), XYZCoord(100, 100, 0), + XYZCoord(0, 100, 0)}}; + Polygon obstacle = {{XYZCoord(40, 40, 0), XYZCoord(60, 40, 0), XYZCoord(60, 60, 0), + XYZCoord(40, 60, 0)}}; + Environment::init(field, {}, {}, {obstacle}); +} + +// Reference implementation: walk the arc point by point. Slow and dumb on purpose -- +// this is what the analytic check is verified against. +bool arcInBoundsBruteForce(const XYZCoord& center, double radius, double start_angle, double sweep, + int steps = 4000) { + for (int i = 0; i <= steps; i++) { + const double angle = start_angle + sweep * (static_cast(i) / steps); + if (!Environment::isPointInBounds( + XYZCoord{center.x + radius * std::cos(angle), + center.y + radius * std::sin(angle), 0})) { + return false; + } + } + return true; +} + +// Reference implementation: generate the whole Dubins curve and check every point. +// This is the check isDubinsPathInBounds replaces. +// +// The generated points do not describe the straightaway -- it is handed back as +// its two endpoints so that ardupilot accelerates through it -- so the legs in +// between are walked as well, otherwise the reference would miss anything the +// plane flies over in a straight line. +bool dubinsInBoundsBruteForce(const RRTPoint& start, const RRTPoint& end, const RRTOption& option) { + if (!std::isfinite(option.length)) { + return false; + } + + const std::vector points = + Dubins::generatePoints(start, end, option.dubins_path, option.has_straight); + + for (std::size_t i = 0; i < points.size(); i++) { + if (!Environment::isPointInBounds(points[i])) { + return false; + } + + if (i == 0) { + continue; + } + + const XYZCoord& previous = points[i - 1]; + const int steps = + std::ceil(previous.distanceTo(points[i]) / Dubins::_point_separation); + for (int step = 1; step < steps; step++) { + const double ratio = static_cast(step) / steps; + if (!Environment::isPointInBounds(previous + ratio * (points[i] - previous))) { + return false; + } + } + } + + return true; +} + +} // namespace + +/* + * Environment::doesArcIntersectSegment -- a segment that never reaches the circle + */ +TEST(DubinsBoundsTest, ArcSegmentMissesCircleEntirely) { + initOpenField(); + const XYZCoord center(0, 0, 0); + + // circle of radius 10, segment sitting out at x = 20 + EXPECT_FALSE(Environment::doesArcIntersectSegment(center, 10, 0, TWO_PI, XYZCoord(20, -20, 0), + XYZCoord(20, 20, 0))); + // segment pointing at the circle but stopping short + EXPECT_FALSE(Environment::doesArcIntersectSegment(center, 10, 0, TWO_PI, XYZCoord(11, 0, 0), + XYZCoord(50, 0, 0))); +} + +/* + * Environment::doesArcIntersectSegment -- the angular window is respected + * + * The vertical segment x = 5 cuts the radius-10 circle at +-60 degrees. + */ +TEST(DubinsBoundsTest, ArcSegmentRespectsSweepWindow) { + initOpenField(); + const XYZCoord center(0, 0, 0); + const XYZCoord seg_start(5, -20, 0); + const XYZCoord seg_end(5, 20, 0); + + // 0 -> 90 deg contains the +60 deg hit + EXPECT_TRUE( + Environment::doesArcIntersectSegment(center, 10, 0, HALF_PI, seg_start, seg_end)); + // 90 -> 180 deg contains neither hit + EXPECT_FALSE( + Environment::doesArcIntersectSegment(center, 10, HALF_PI, HALF_PI, seg_start, seg_end)); + // 180 -> 270 deg contains neither hit + EXPECT_FALSE( + Environment::doesArcIntersectSegment(center, 10, M_PI, HALF_PI, seg_start, seg_end)); + // the full circle always contains both + EXPECT_TRUE(Environment::doesArcIntersectSegment(center, 10, 0, TWO_PI, seg_start, seg_end)); +} + +/* + * Environment::doesArcIntersectSegment -- sweep sign selects the direction travelled + */ +TEST(DubinsBoundsTest, ArcSegmentRespectsSweepDirection) { + initOpenField(); + const XYZCoord center(0, 0, 0); + const XYZCoord seg_start(5, -20, 0); + const XYZCoord seg_end(5, 20, 0); + + // starting at 0 and turning CCW a quarter turn reaches +60 deg, but not -60 deg + EXPECT_TRUE(Environment::doesArcIntersectSegment(center, 10, 0, HALF_PI, seg_start, seg_end)); + // starting at 0 and turning CW a quarter turn reaches -60 deg, but not +60 deg + EXPECT_TRUE(Environment::doesArcIntersectSegment(center, 10, 0, -HALF_PI, seg_start, seg_end)); + + // a segment that only crosses on the +y side, to tell the two directions apart + const XYZCoord upper_start(-20, 9, 0); + const XYZCoord upper_end(20, 9, 0); + EXPECT_TRUE( + Environment::doesArcIntersectSegment(center, 10, 0, HALF_PI, upper_start, upper_end)); + EXPECT_FALSE( + Environment::doesArcIntersectSegment(center, 10, 0, -HALF_PI, upper_start, upper_end)); +} + +/* + * Environment::doesArcIntersectSegment -- degenerate and boundary inputs + */ +TEST(DubinsBoundsTest, ArcSegmentEdgeCases) { + initOpenField(); + const XYZCoord center(0, 0, 0); + + // segment entirely inside the circle never touches it + EXPECT_FALSE(Environment::doesArcIntersectSegment(center, 10, 0, TWO_PI, XYZCoord(-1, -1, 0), + XYZCoord(1, 1, 0))); + + // zero length segment + EXPECT_FALSE(Environment::doesArcIntersectSegment(center, 10, 0, TWO_PI, XYZCoord(10, 0, 0), + XYZCoord(10, 0, 0))); + + // tangent line y = 10 touches at exactly one point, (0, 10). Counted as a hit + // (conservative), and only when the sweep covers 90 deg + EXPECT_TRUE(Environment::doesArcIntersectSegment(center, 10, 0, M_PI, XYZCoord(-20, 10, 0), + XYZCoord(20, 10, 0))); + EXPECT_FALSE(Environment::doesArcIntersectSegment(center, 10, M_PI, HALF_PI, + XYZCoord(-20, 10, 0), XYZCoord(20, 10, 0))); + + // segment whose endpoint lies exactly on the circle (t == 0) + EXPECT_TRUE(Environment::doesArcIntersectSegment(center, 10, 0, HALF_PI, XYZCoord(10, 0, 0), + XYZCoord(30, 0, 0))); + + // a zero sweep arc is a single point, so it only touches the segment when that exact + // point lies on it -- here, the tangent point at 90 deg + EXPECT_TRUE(Environment::doesArcIntersectSegment(center, 10, HALF_PI, 0, XYZCoord(-20, 10, 0), + XYZCoord(20, 10, 0))); + // ... and not when the arc collapses anywhere else on the circle + EXPECT_FALSE(Environment::doesArcIntersectSegment(center, 10, 0, 0, XYZCoord(-20, 10, 0), + XYZCoord(20, 10, 0))); +} + +/* + * Environment::isArcInBounds -- clear of everything + */ +TEST(DubinsBoundsTest, ArcInsideOpenField) { + initOpenField(); + + // full circle well inside the field + EXPECT_TRUE(Environment::isArcInBounds(XYZCoord(50, 50, 0), 20, 0, TWO_PI)); + // small arcs anywhere in the interior + EXPECT_TRUE(Environment::isArcInBounds(XYZCoord(20, 20, 0), 10, 0, HALF_PI)); + EXPECT_TRUE(Environment::isArcInBounds(XYZCoord(80, 80, 0), 10, M_PI, -M_PI)); +} + +/* + * Environment::isArcInBounds -- leaving the valid region + */ +TEST(DubinsBoundsTest, ArcLeavingRegionIsRejected) { + initOpenField(); + + // circle centered near the left edge, bulging out past x = 0 + EXPECT_FALSE(Environment::isArcInBounds(XYZCoord(5, 50, 0), 10, 0, TWO_PI)); + // the same circle, swept only on the side that stays inside + EXPECT_TRUE(Environment::isArcInBounds(XYZCoord(5, 50, 0), 10, -HALF_PI, M_PI)); + + // arc that starts outside the region entirely + EXPECT_FALSE(Environment::isArcInBounds(XYZCoord(-50, 50, 0), 10, 0, HALF_PI)); +} + +/* + * Environment::isArcInBounds -- obstacles + */ +TEST(DubinsBoundsTest, ArcAndObstacle) { + initFieldWithObstacle(); + + // circle entirely inside the obstacle -- caught by the containment check + EXPECT_FALSE(Environment::isArcInBounds(XYZCoord(50, 50, 0), 5, 0, TWO_PI)); + + // circle that starts clear of the obstacle but cuts through it + EXPECT_TRUE(Environment::isPointInBounds(XYZCoord(65, 30, 0))); // arc start is clear + EXPECT_FALSE(Environment::isArcInBounds(XYZCoord(50, 30, 0), 15, 0, TWO_PI)); + + // same circle swept CCW from 180 through 270 deg, staying below y = 30 and so + // never reaching the obstacle at y = 40 + EXPECT_TRUE(Environment::isArcInBounds(XYZCoord(50, 30, 0), 15, M_PI, M_PI)); + + // circle sitting in the corner of the field, nowhere near the obstacle + EXPECT_TRUE(Environment::isArcInBounds(XYZCoord(15, 15, 0), 10, 0, TWO_PI)); +} + +/* + * Environment::isArcInBounds -- differential against a densely sampled arc + */ +TEST(DubinsBoundsTest, ArcMatchesSampledReference) { + initFieldWithObstacle(); + + std::mt19937 gen(1337); + std::uniform_real_distribution coord(0, 100); + std::uniform_real_distribution radius(1, 40); + std::uniform_real_distribution angle(-TWO_PI, TWO_PI); + + int analytic_accepted = 0; + for (int i = 0; i < 3000; i++) { + const XYZCoord center(coord(gen), coord(gen), 0); + const double r = radius(gen); + const double start_angle = angle(gen); + const double sweep = angle(gen); + + const bool analytic = Environment::isArcInBounds(center, r, start_angle, sweep); + if (analytic) { + analytic_accepted++; + // the only direction that matters: never accept an arc that leaves bounds + EXPECT_TRUE(arcInBoundsBruteForce(center, r, start_angle, sweep)) + << "accepted an out-of-bounds arc: center (" << center.x << ", " << center.y + << ") r " << r << " from " << start_angle << " sweeping " << sweep; + } + } + + // guards against the check having degenerated into "always false" + EXPECT_GT(analytic_accepted, 300); +} + +/* + * Environment::isDubinsPathInBounds -- options that are rejected without any geometry + */ +TEST(DubinsBoundsTest, RejectsUnusableOptions) { + initOpenField(); + Dubins::_radius = 5; + Dubins::_point_separation = 0.5; + + const RRTPoint start(XYZCoord(50, 50, 0), 0); + const RRTPoint end(XYZCoord(70, 50, 0), 0); + + // lsr/rsl hand back infinity when the turning circles overlap + const RRTOption infinite{std::numeric_limits::infinity(), DubinsPath(0, 0, 0), true}; + EXPECT_FALSE(Environment::isDubinsPathInBounds(start, end, infinite)); + + // CCC paths [LRL, RLR] are not handled analytically and are rejected outright + const RRTOption curve_only{10, DubinsPath(1, 1, 1), false}; + EXPECT_FALSE(Environment::isDubinsPathInBounds(start, end, curve_only)); +} + +/* + * Environment::isDubinsPathInBounds -- endpoints outside the region + */ +TEST(DubinsBoundsTest, RejectsOutOfBoundsEndpoints) { + initOpenField(); + Dubins::_radius = 5; + Dubins::_point_separation = 0.5; + + const RRTPoint inside(XYZCoord(50, 50, 0), 0); + const RRTPoint outside(XYZCoord(150, 50, 0), 0); + + for (const RRTOption& option : Dubins::allOptions(outside, inside)) { + EXPECT_FALSE(Environment::isDubinsPathInBounds(outside, inside, option)); + } + for (const RRTOption& option : Dubins::allOptions(inside, outside)) { + EXPECT_FALSE(Environment::isDubinsPathInBounds(inside, outside, option)); + } +} + +/* + * Environment::isDubinsPathInBounds -- short hops across open space are accepted + */ +TEST(DubinsBoundsTest, AcceptsPathInOpenField) { + initOpenField(); + Dubins::_radius = 5; + Dubins::_point_separation = 0.1; + + const RRTPoint start(XYZCoord(40, 50, 0), 0); + const RRTPoint end(XYZCoord(60, 50, 0), 0); + + int accepted = 0; + for (const RRTOption& option : Dubins::allOptions(start, end)) { + if (Environment::isDubinsPathInBounds(start, end, option)) { + accepted++; + EXPECT_TRUE(dubinsInBoundsBruteForce(start, end, option)); + } + } + + // a straight shot down the middle of an empty field: at least one option must work + EXPECT_GT(accepted, 0); +} + +/* + * Environment::isDubinsPathInBounds -- a turn that bulges out of the region + * + * This is the case point-sampling was there to catch. Both endpoints are in bounds + * and the straight section is in bounds, but with a 20 unit turning radius the plane + * has to swing below y = 0 to reverse direction if it turns right. + */ +TEST(DubinsBoundsTest, RejectsTurnBulgingOutOfRegion) { + initOpenField(); + Dubins::_radius = 20; + Dubins::_point_separation = 0.05; + + const RRTPoint start(XYZCoord(50, 10, 0), 0); // heading +x, near the bottom edge + const RRTPoint end(XYZCoord(50, 30, 0), M_PI); // heading -x + + ASSERT_TRUE(Environment::isPointInBounds(start.coord)); + ASSERT_TRUE(Environment::isPointInBounds(end.coord)); + + // turning right puts the first turn's center at (50, -10), dragging the arc + // below the bottom edge + const RRTOption right = Dubins::rsr(start, end, Dubins::findCenter(start, 'R'), + Dubins::findCenter(end, 'R')); + EXPECT_FALSE(Environment::isDubinsPathInBounds(start, end, right)); + EXPECT_FALSE(dubinsInBoundsBruteForce(start, end, right)); + + // reversing heading with a 20 unit radius needs ~100 units of room, so every option + // here leaves the field somewhere -- the analytic verdict must track the sampled one + for (const RRTOption& option : Dubins::allOptions(start, end)) { + if (!std::isfinite(option.length)) { + continue; + } + EXPECT_EQ(Environment::isDubinsPathInBounds(start, end, option), + dubinsInBoundsBruteForce(start, end, option)); + } + + // the identical maneuver with a tight turning radius fits inside the field + Dubins::_radius = 3; + const RRTOption tight = Dubins::rsr(start, end, Dubins::findCenter(start, 'R'), + Dubins::findCenter(end, 'R')); + EXPECT_TRUE(Environment::isDubinsPathInBounds(start, end, tight)); + EXPECT_TRUE(dubinsInBoundsBruteForce(start, end, tight)); +} + +/* + * Environment::isDubinsPathInBounds -- paths through an obstacle + */ +TEST(DubinsBoundsTest, RejectsPathThroughObstacle) { + initFieldWithObstacle(); + Dubins::_radius = 5; + Dubins::_point_separation = 0.05; + + // straight across the middle, which runs directly through [40, 60] x [40, 60] + const RRTPoint start(XYZCoord(10, 50, 0), 0); + const RRTPoint end(XYZCoord(90, 50, 0), 0); + + for (const RRTOption& option : Dubins::allOptions(start, end)) { + EXPECT_FALSE(Environment::isDubinsPathInBounds(start, end, option)) + << "accepted a path straight through the obstacle"; + } + + // the same hop, moved down to y = 20 where the obstacle is not in the way + const RRTPoint clear_start(XYZCoord(10, 20, 0), 0); + const RRTPoint clear_end(XYZCoord(90, 20, 0), 0); + + int accepted = 0; + for (const RRTOption& option : Dubins::allOptions(clear_start, clear_end)) { + if (Environment::isDubinsPathInBounds(clear_start, clear_end, option)) { + accepted++; + } + } + EXPECT_GT(accepted, 0); +} + +/* + * Environment::isDubinsPathInBounds -- differential against the sampled check, + * over every option Dubins::allOptions produces for random start/end pairs + */ +TEST(DubinsBoundsTest, MatchesSampledReference) { + initFieldWithObstacle(); + Dubins::_radius = 5; + Dubins::_point_separation = 0.02; // fine enough for the reference to be trusted + + std::mt19937 gen(42); + std::uniform_real_distribution position(2, 98); + std::uniform_real_distribution heading(0, TWO_PI); + + int total = 0; + int agree = 0; + int accepted = 0; + + for (int i = 0; i < 750; i++) { + const RRTPoint start(XYZCoord(position(gen), position(gen), 0), heading(gen)); + const RRTPoint end(XYZCoord(position(gen), position(gen), 0), heading(gen)); + + for (const RRTOption& option : Dubins::allOptions(start, end)) { + if (!std::isfinite(option.length)) { + continue; + } + + const bool analytic = Environment::isDubinsPathInBounds(start, end, option); + const bool sampled = dubinsInBoundsBruteForce(start, end, option); + + total++; + agree += (analytic == sampled); + accepted += analytic; + + // the check may reject a path the sampler accepts (grazing a boundary between + // samples), but must never accept one the sampler rejects + EXPECT_FALSE(analytic && !sampled) + << "accepted an out-of-bounds path: start (" << start.coord.x << ", " + << start.coord.y << ") psi " << start.psi << " -> end (" << end.coord.x << ", " + << end.coord.y << ") psi " << end.psi; + } + } + + ASSERT_GT(total, 0); + // conservative rejections are fine, but they should be rare + EXPECT_GT(static_cast(agree) / total, 0.99); + // and the check must not have degenerated into "always false" + EXPECT_GT(accepted, total / 10); +} + +/* + * Environment::isDubinsPathInBounds -- holds up across turning radii and obstacle layouts + */ +TEST(DubinsBoundsTest, MatchesSampledReferenceAcrossConfigurations) { + Polygon field = {{XYZCoord(0, 0, 0), XYZCoord(100, 0, 0), XYZCoord(100, 100, 0), + XYZCoord(0, 100, 0)}}; + + // a thin wall, a corner block, and a pair of pillars + const std::vector> obstacle_sets = { + {}, + {{{XYZCoord(48, 10, 0), XYZCoord(52, 10, 0), XYZCoord(52, 90, 0), XYZCoord(48, 90, 0)}}}, + {{{XYZCoord(0, 0, 0), XYZCoord(30, 0, 0), XYZCoord(30, 30, 0), XYZCoord(0, 30, 0)}}}, + {{{XYZCoord(20, 20, 0), XYZCoord(35, 20, 0), XYZCoord(35, 35, 0), XYZCoord(20, 35, 0)}}, + {{XYZCoord(65, 65, 0), XYZCoord(80, 65, 0), XYZCoord(80, 80, 0), XYZCoord(65, 80, 0)}}}, + }; + + std::mt19937 gen(2024); + std::uniform_real_distribution position(2, 98); + std::uniform_real_distribution heading(0, TWO_PI); + + for (const double turn_radius : {2.0, 5.0, 15.0, 30.0}) { + for (const std::vector& obstacles : obstacle_sets) { + Environment::init(field, {}, {}, obstacles); + Dubins::_radius = turn_radius; + Dubins::_point_separation = 0.02; + + for (int i = 0; i < 120; i++) { + const RRTPoint start(XYZCoord(position(gen), position(gen), 0), heading(gen)); + const RRTPoint end(XYZCoord(position(gen), position(gen), 0), heading(gen)); + + for (const RRTOption& option : Dubins::allOptions(start, end)) { + if (!std::isfinite(option.length)) { + continue; + } + + if (Environment::isDubinsPathInBounds(start, end, option)) { + EXPECT_TRUE(dubinsInBoundsBruteForce(start, end, option)) + << "radius " << turn_radius << ", " << obstacles.size() + << " obstacle(s): accepted an out-of-bounds path from (" + << start.coord.x << ", " << start.coord.y << ") to (" << end.coord.x + << ", " << end.coord.y << ")"; + } + } + } + } + } +} + +/* + * ============================================================================ + * The rest of the environment: region bounds, line checks, sampling, and the + * airdrop coverage helpers + * ============================================================================ + */ + +/* + * Environment::findBounds -- the box a region sits in + */ +TEST(EnvironmentTest, FindBounds) { + const Polygon square = { + {XYZCoord(0, 0, 0), XYZCoord(100, 0, 0), XYZCoord(100, 50, 0), XYZCoord(0, 50, 0)}}; + const auto square_bounds = Environment::findBounds(square); + EXPECT_DOUBLE_EQ(square_bounds.first.first, 0); // min x + EXPECT_DOUBLE_EQ(square_bounds.first.second, 100); // max x + EXPECT_DOUBLE_EQ(square_bounds.second.first, 0); // min y + EXPECT_DOUBLE_EQ(square_bounds.second.second, 50); // max y + + // a region that reaches into the negatives, and whose extremes are on + // different vertices + const Polygon triangle = { + {XYZCoord(-10, -5, 0), XYZCoord(4, 20, 0), XYZCoord(-2, -30, 0)}}; + const auto triangle_bounds = Environment::findBounds(triangle); + EXPECT_DOUBLE_EQ(triangle_bounds.first.first, -10); + EXPECT_DOUBLE_EQ(triangle_bounds.first.second, 4); + EXPECT_DOUBLE_EQ(triangle_bounds.second.first, -30); + EXPECT_DOUBLE_EQ(triangle_bounds.second.second, 20); + + // a single point is its own box + const auto point_bounds = Environment::findBounds({XYZCoord(7, -3, 0)}); + EXPECT_DOUBLE_EQ(point_bounds.first.first, 7); + EXPECT_DOUBLE_EQ(point_bounds.first.second, 7); + EXPECT_DOUBLE_EQ(point_bounds.second.first, -3); + EXPECT_DOUBLE_EQ(point_bounds.second.second, -3); + + // an empty region has nothing to bound + const auto empty_bounds = Environment::findBounds({}); + EXPECT_DOUBLE_EQ(empty_bounds.first.first, 0); + EXPECT_DOUBLE_EQ(empty_bounds.first.second, 0); + EXPECT_DOUBLE_EQ(empty_bounds.second.first, 0); + EXPECT_DOUBLE_EQ(empty_bounds.second.second, 0); +} + +/* + * Environment::scale -- grows or shrinks a region about the center of its box + */ +TEST(EnvironmentTest, ScalePolygon) { + // centered on (5, 5) + const Polygon square = { + {XYZCoord(0, 0, 0), XYZCoord(10, 0, 0), XYZCoord(10, 10, 0), XYZCoord(0, 10, 0)}}; + + const Polygon doubled = Environment::scale(2, square); + const Polygon expected_doubled = { + {XYZCoord(-5, -5, 0), XYZCoord(15, -5, 0), XYZCoord(15, 15, 0), XYZCoord(-5, 15, 0)}}; + EXPECT_EQ(doubled, expected_doubled); + + const Polygon halved = Environment::scale(0.5, square); + const Polygon expected_halved = {{XYZCoord(2.5, 2.5, 0), XYZCoord(7.5, 2.5, 0), + XYZCoord(7.5, 7.5, 0), XYZCoord(2.5, 7.5, 0)}}; + EXPECT_EQ(halved, expected_halved); + + // scaling by one changes nothing + EXPECT_EQ(Environment::scale(1, square), square); + + // an off center region is scaled about its own box, so the box keeps its center + const Polygon triangle = {{XYZCoord(20, 40, 0), XYZCoord(60, 40, 0), XYZCoord(40, 80, 0)}}; + const Polygon scaled_triangle = Environment::scale(3, triangle); + ASSERT_EQ(scaled_triangle.size(), triangle.size()); + + const auto before = Environment::findBounds(triangle); + const auto after = Environment::findBounds(scaled_triangle); + EXPECT_DOUBLE_EQ((after.first.first + after.first.second) / 2, + (before.first.first + before.first.second) / 2); + EXPECT_DOUBLE_EQ((after.second.first + after.second.second) / 2, + (before.second.first + before.second.second) / 2); + EXPECT_DOUBLE_EQ(after.first.second - after.first.first, + 3 * (before.first.second - before.first.first)); +} + +/* + * Environment::doesLineIntersectPolygon -- a segment against every edge + */ +TEST(EnvironmentTest, DoesLineIntersectPolygon) { + const Polygon obstacle = { + {XYZCoord(40, 40, 0), XYZCoord(60, 40, 0), XYZCoord(60, 60, 0), XYZCoord(40, 60, 0)}}; + Environment::init({}, {}, {}); + + // straight through + EXPECT_TRUE( + Environment::doesLineIntersectPolygon(XYZCoord(0, 50, 0), XYZCoord(100, 50, 0), obstacle)); + // clear of it + EXPECT_FALSE( + Environment::doesLineIntersectPolygon(XYZCoord(0, 10, 0), XYZCoord(100, 10, 0), obstacle)); + // stops short of it + EXPECT_FALSE( + Environment::doesLineIntersectPolygon(XYZCoord(0, 50, 0), XYZCoord(39, 50, 0), obstacle)); + // a segment entirely inside crosses no edge + EXPECT_FALSE( + Environment::doesLineIntersectPolygon(XYZCoord(45, 45, 0), XYZCoord(55, 55, 0), obstacle)); + // touching an edge counts, the checks are conservative + EXPECT_TRUE( + Environment::doesLineIntersectPolygon(XYZCoord(0, 40, 0), XYZCoord(50, 40, 0), obstacle)); +} + +/* + * Environment::isLineInBounds -- the region and every obstacle at once + */ +TEST(EnvironmentTest, LineInBounds) { + initFieldWithObstacle(); + + // well clear of everything + EXPECT_TRUE(Environment::isLineInBounds(XYZCoord(10, 10, 0), XYZCoord(30, 30, 0))); + // passing under the obstacle + EXPECT_TRUE(Environment::isLineInBounds(XYZCoord(10, 10, 0), XYZCoord(90, 10, 0))); + + // straight through the obstacle + EXPECT_FALSE(Environment::isLineInBounds(XYZCoord(10, 50, 0), XYZCoord(90, 50, 0))); + // clipping the corner of it + EXPECT_FALSE(Environment::isLineInBounds(XYZCoord(10, 10, 0), XYZCoord(40, 40, 0))); + // leaving the field + EXPECT_FALSE(Environment::isLineInBounds(XYZCoord(50, 10, 0), XYZCoord(150, 10, 0))); +} + +/* + * Environment::getRandomPoint -- samples land somewhere they can be flown + */ +TEST(EnvironmentTest, GetRandomPoint) { + initFieldWithObstacle(); + const XYZCoord fallback(1, 1, 0); + + for (int i = 0; i < 500; i++) { + const XYZCoord point = Environment::getRandomPoint(false, fallback); + EXPECT_TRUE(Environment::isPointInBounds(point)) + << "sampled (" << point.x << ", " << point.y << ")"; + } + + // sampling the mapping region only has to land in the mapping region + const Polygon field = {{XYZCoord(0, 0, 0), XYZCoord(100, 0, 0), XYZCoord(100, 100, 0), + XYZCoord(0, 100, 0)}}; + const Polygon mapping_region = {{XYZCoord(10, 10, 0), XYZCoord(30, 10, 0), + XYZCoord(30, 30, 0), XYZCoord(10, 30, 0)}}; + Environment::init(field, {}, mapping_region, {}); + + for (int i = 0; i < 500; i++) { + const XYZCoord point = Environment::getRandomPoint(true, fallback); + EXPECT_TRUE(Environment::isPointInPolygon(mapping_region, point)) + << "sampled (" << point.x << ", " << point.y << ")"; + } + + // nowhere to sample from, so the caller's fallback is handed back + Environment::init({}, {}, {}, {}); + EXPECT_TRUE(Environment::getRandomPoint(false, fallback) == fallback); + EXPECT_TRUE(Environment::getRandomPoint(true, fallback) == fallback); +} + +/* + * Environment::orientation and Environment::onSegment, the two helpers the + * segment intersection check is built out of + */ +TEST(EnvironmentTest, OrientationAndOnSegment) { + Environment::init({}, {}, {}); + + // 0 colinear, 1 clockwise, 2 counterclockwise + EXPECT_EQ(Environment::orientation(XYZCoord(0, 0, 0), XYZCoord(1, 1, 0), XYZCoord(2, 2, 0)), 0); + EXPECT_EQ(Environment::orientation(XYZCoord(0, 0, 0), XYZCoord(1, 1, 0), XYZCoord(2, 0, 0)), 1); + EXPECT_EQ(Environment::orientation(XYZCoord(0, 0, 0), XYZCoord(1, 1, 0), XYZCoord(0, 2, 0)), 2); + // colinear along an axis, and with a repeated point + EXPECT_EQ(Environment::orientation(XYZCoord(0, 0, 0), XYZCoord(5, 0, 0), XYZCoord(9, 0, 0)), 0); + EXPECT_EQ(Environment::orientation(XYZCoord(0, 0, 0), XYZCoord(0, 0, 0), XYZCoord(9, 3, 0)), 0); + + // onSegment(p, q, r) asks whether q is inside the box p and r span + EXPECT_TRUE(Environment::onSegment(XYZCoord(0, 0, 0), XYZCoord(1, 1, 0), XYZCoord(2, 2, 0))); + EXPECT_TRUE(Environment::onSegment(XYZCoord(0, 0, 0), XYZCoord(0, 0, 0), XYZCoord(2, 2, 0))); + EXPECT_TRUE(Environment::onSegment(XYZCoord(2, 2, 0), XYZCoord(1, 1, 0), XYZCoord(0, 0, 0))); + EXPECT_FALSE(Environment::onSegment(XYZCoord(0, 0, 0), XYZCoord(3, 3, 0), XYZCoord(2, 2, 0))); + EXPECT_FALSE(Environment::onSegment(XYZCoord(0, 0, 0), XYZCoord(-1, 1, 0), XYZCoord(2, 2, 0))); +} + +/* + * Environment::horizontalRayIntersectsEdge + */ +TEST(EnvironmentTest, HorizontalRayIntersectsEdge) { + const Polygon airdrop_zone = { + {XYZCoord(0, 0, 0), XYZCoord(100, 0, 0), XYZCoord(100, 100, 0), XYZCoord(50, 100, 0)}}; + Environment::init({}, airdrop_zone, {}); + + const XYZCoord ray_start(-9999, 75, 0); + const XYZCoord ray_end(9999, 75, 0); + XYZCoord intersection(0, 0, 0); + + // the bottom edge is nowhere near the ray + EXPECT_FALSE(Environment::horizontalRayIntersectsEdge(airdrop_zone[0], airdrop_zone[1], + ray_start, ray_end, intersection)); + // the right edge is vertical, so it is hit at its own x + EXPECT_TRUE(Environment::horizontalRayIntersectsEdge(airdrop_zone[1], airdrop_zone[2], + ray_start, ray_end, intersection)); + EXPECT_EQ(intersection, XYZCoord(100, 75, 0)); + // the top edge sits above the ray + EXPECT_FALSE(Environment::horizontalRayIntersectsEdge(airdrop_zone[2], airdrop_zone[3], + ray_start, ray_end, intersection)); + // the slanted edge is hit part way along + EXPECT_TRUE(Environment::horizontalRayIntersectsEdge(airdrop_zone[3], airdrop_zone[0], + ray_start, ray_end, intersection)); + EXPECT_EQ(intersection, XYZCoord(37.5, 75, 0)); +} + +/* + * Environment::findIntersections -- where a ray crosses a region, either way up + */ +TEST(EnvironmentTest, FindIntersections) { + const Polygon airdrop_zone = { + {XYZCoord(0, 0, 0), XYZCoord(100, 0, 0), XYZCoord(100, 100, 0), XYZCoord(50, 100, 0)}}; + Environment::init({}, airdrop_zone, {}); + + const std::vector vertical = Environment::findIntersections( + airdrop_zone, XYZCoord(75, 9999, 0), XYZCoord(75, -9999, 0), true); + EXPECT_EQ(vertical, std::vector({XYZCoord(75, 0, 0), XYZCoord(75, 100, 0)})); + + const std::vector horizontal = Environment::findIntersections( + airdrop_zone, XYZCoord(-9999, 75, 0), XYZCoord(9999, 75, 0), false); + EXPECT_EQ(horizontal, std::vector({XYZCoord(100, 75, 0), XYZCoord(37.5, 75, 0)})); + + // a ray that misses the region entirely + EXPECT_TRUE(Environment::findIntersections(airdrop_zone, XYZCoord(200, 9999, 0), + XYZCoord(200, -9999, 0), true) + .empty()); +} + +/* + * Environment::getAirdropEndpoints -- the ends of the scan lines that cover the + * airdrop zone, one scan_radius in from the edge and 2 * scan_radius apart + */ +TEST(EnvironmentTest, GetAirdropEndpoints) { + const Polygon airdrop_zone = { + {XYZCoord(0, 0, 0), XYZCoord(100, 0, 0), XYZCoord(100, 100, 0), XYZCoord(0, 100, 0)}}; + Environment::init({}, airdrop_zone, {}); + + // horizontal lines, handed back top down and left right + const std::vector horizontal = Environment::getAirdropEndpoints(25, false); + EXPECT_EQ(horizontal, std::vector({XYZCoord(0, 75, 0), XYZCoord(100, 75, 0), + XYZCoord(0, 25, 0), XYZCoord(100, 25, 0)})); + + // vertical lines, handed back left right and top down + const std::vector vertical = Environment::getAirdropEndpoints(25, true); + EXPECT_EQ(vertical, std::vector({XYZCoord(25, 100, 0), XYZCoord(25, 0, 0), + XYZCoord(75, 100, 0), XYZCoord(75, 0, 0)})); +} + +/* + * Environment::getAirdropWaypoints -- the same lines, with the heading they are + * flown at + */ +TEST(EnvironmentTest, GetAirdropWaypoints) { + const Polygon airdrop_zone = { + {XYZCoord(0, 0, 0), XYZCoord(100, 0, 0), XYZCoord(100, 100, 0), XYZCoord(0, 100, 0)}}; + Environment::init({}, airdrop_zone, {}); + + // flying every line the same way, so the plane has to fly back between them + const std::vector one_way = Environment::getAirdropWaypoints(25, true, false); + ASSERT_EQ(one_way.size(), 4); + for (const RRTPoint& waypoint : one_way) { + EXPECT_DOUBLE_EQ(waypoint.psi, 0); + } + EXPECT_TRUE(one_way[0].coord == XYZCoord(0, 75, 0)); + EXPECT_TRUE(one_way[1].coord == XYZCoord(100, 75, 0)); + EXPECT_TRUE(one_way[2].coord == XYZCoord(0, 25, 0)); + EXPECT_TRUE(one_way[3].coord == XYZCoord(100, 25, 0)); + + // boustrophedon: every other line is flown backwards, so the ends swap and + // the heading turns around with them + const std::vector alternating = Environment::getAirdropWaypoints(25, false, false); + ASSERT_EQ(alternating.size(), 4); + EXPECT_TRUE(alternating[0].coord == XYZCoord(0, 75, 0)); + EXPECT_TRUE(alternating[1].coord == XYZCoord(100, 75, 0)); + EXPECT_TRUE(alternating[2].coord == XYZCoord(100, 25, 0)); + EXPECT_TRUE(alternating[3].coord == XYZCoord(0, 25, 0)); + EXPECT_DOUBLE_EQ(alternating[0].psi, 0); + EXPECT_DOUBLE_EQ(alternating[1].psi, 0); + EXPECT_DOUBLE_EQ(alternating[2].psi, M_PI); + EXPECT_DOUBLE_EQ(alternating[3].psi, M_PI); + + // scanning vertically instead, which is flown south + const std::vector vertical = Environment::getAirdropWaypoints(25, true, true); + ASSERT_EQ(vertical.size(), 4); + EXPECT_TRUE(vertical[0].coord == XYZCoord(25, 100, 0)); + EXPECT_TRUE(vertical[1].coord == XYZCoord(25, 0, 0)); + for (const RRTPoint& waypoint : vertical) { + EXPECT_DOUBLE_EQ(waypoint.psi, 3 * HALF_PI); + } +} + +/* + * Environment::isPolygonInPolygon -- a region is only inside another one if all + * of it is + */ +TEST(EnvironmentTest, PolygonInPolygon) { + // 100 x 100 field + const Polygon field = {XYZCoord{0, 0, 0}, XYZCoord{100, 0, 0}, XYZCoord{100, 100, 0}, + XYZCoord{0, 100, 0}}; + + // well clear of every edge + const Polygon inside = {XYZCoord{10, 10, 0}, XYZCoord{40, 10, 0}, XYZCoord{40, 40, 0}, + XYZCoord{10, 40, 0}}; + EXPECT_TRUE(Environment::isPolygonInPolygon(inside, field)); + + // hanging off of the right side + const Polygon overlapping = {XYZCoord{80, 10, 0}, XYZCoord{120, 10, 0}, XYZCoord{120, 40, 0}, + XYZCoord{80, 40, 0}}; + EXPECT_FALSE(Environment::isPolygonInPolygon(overlapping, field)); + + // nowhere near it + const Polygon outside = {XYZCoord{200, 200, 0}, XYZCoord{240, 200, 0}, XYZCoord{240, 240, 0}, + XYZCoord{200, 240, 0}}; + EXPECT_FALSE(Environment::isPolygonInPolygon(outside, field)); + + // the field does not fit inside of the region it contains + EXPECT_FALSE(Environment::isPolygonInPolygon(field, inside)); + + // sharing an edge is not being inside of it, the same way a point on the edge + // is not in the polygon + const Polygon flush = {XYZCoord{0, 0, 0}, XYZCoord{50, 0, 0}, XYZCoord{50, 50, 0}, + XYZCoord{0, 50, 0}}; + EXPECT_FALSE(Environment::isPolygonInPolygon(flush, field)); + + // every corner is inside, but the middle of the region bulges out of the notch + const Polygon notched = {XYZCoord{0, 0, 0}, XYZCoord{100, 0, 0}, XYZCoord{100, 100, 0}, + XYZCoord{60, 100, 0}, XYZCoord{60, 20, 0}, XYZCoord{40, 20, 0}, + XYZCoord{40, 100, 0}, XYZCoord{0, 100, 0}}; + const Polygon spanning_the_notch = {XYZCoord{20, 50, 0}, XYZCoord{80, 50, 0}, + XYZCoord{80, 80, 0}, XYZCoord{20, 80, 0}}; + for (const XYZCoord& corner : spanning_the_notch) { + ASSERT_TRUE(Environment::isPointInPolygon(notched, corner)); + } + EXPECT_FALSE(Environment::isPolygonInPolygon(spanning_the_notch, notched)); + + // a region is inside of itself only in the sense that it is not inside of it + EXPECT_FALSE(Environment::isPolygonInPolygon(field, field)); +} diff --git a/tests/unit/mission_parameters_test.cpp b/tests/unit/mission_parameters_test.cpp new file mode 100644 index 00000000..065d4680 --- /dev/null +++ b/tests/unit/mission_parameters_test.cpp @@ -0,0 +1,114 @@ +#include "core/mission_parameters.hpp" + +#include + +#include +#include + +#include "pathing/cartesian.hpp" +#include "protos/obc.pb.h" +#include "utilities/datatypes.hpp" + +namespace { + +// somewhere over Maryland, a hundredth of a degree is a bit under a kilometer +const double CENTER_LAT = 38.31; +const double CENTER_LNG = -76.55; + +void addCoord(GPSProtoVec* coords, double lat, double lng) { + GPSCoord* coord = coords->Add(); + coord->set_latitude(lat); + coord->set_longitude(lng); + coord->set_altitude(0); +} + +// a square centered on the field, reaching out half_width degrees each way +void addSquare(GPSProtoVec* coords, double half_width) { + addCoord(coords, CENTER_LAT - half_width, CENTER_LNG - half_width); + addCoord(coords, CENTER_LAT - half_width, CENTER_LNG + half_width); + addCoord(coords, CENTER_LAT + half_width, CENTER_LNG + half_width); + addCoord(coords, CENTER_LAT + half_width, CENTER_LNG - half_width); +} + +// a mission that is flyable as it stands, for a test to then break one part of +Mission validMission() { + Mission mission; + addSquare(mission.mutable_flightboundary(), 0.01); + addSquare(mission.mutable_airdropboundary(), 0.005); + addCoord(mission.mutable_waypoints(), CENTER_LAT, CENTER_LNG); + addCoord(mission.mutable_waypoints(), CENTER_LAT + 0.008, CENTER_LNG - 0.008); + return mission; +} + +std::optional upload(const Mission& mission) { + MissionParameters params; + return params.setMission(mission, CartesianConverter(mission.flightboundary())); +} + +} // namespace + +/* + * MissionParameters::setMission -- a mission that stays inside its own flight + * boundary is taken as it is + */ +TEST(MissionParametersTest, MissionInsideTheFlightBoundaryIsAccepted) { + EXPECT_FALSE(upload(validMission()).has_value()); +} + +/* + * MissionParameters::setMission -- the plane is never asked to fly to a waypoint + * it is not allowed to fly to + */ +TEST(MissionParametersTest, WaypointOutsideTheFlightBoundaryIsRejected) { + Mission mission = validMission(); + addCoord(mission.mutable_waypoints(), CENTER_LAT + 0.02, CENTER_LNG); + + const std::optional err = upload(mission); + + ASSERT_TRUE(err.has_value()); + EXPECT_NE(err->find("Waypoint 3"), std::string::npos) << err.value(); + EXPECT_NE(err->find("flight boundary"), std::string::npos) << err.value(); +} + +/* + * MissionParameters::setMission -- an airdrop zone reaching outside of the flight + * boundary would send the plane out of it + */ +TEST(MissionParametersTest, AirdropBoundaryOutsideTheFlightBoundaryIsRejected) { + // bigger than the flight boundary it is supposed to sit inside of + Mission swallowing = validMission(); + swallowing.clear_airdropboundary(); + addSquare(swallowing.mutable_airdropboundary(), 0.02); + + const std::optional swallowing_err = upload(swallowing); + ASSERT_TRUE(swallowing_err.has_value()); + EXPECT_NE(swallowing_err->find("Airdrop boundary"), std::string::npos) + << swallowing_err.value(); + + // hanging off of one side of it + Mission overlapping = validMission(); + overlapping.clear_airdropboundary(); + addCoord(overlapping.mutable_airdropboundary(), CENTER_LAT - 0.005, CENTER_LNG); + addCoord(overlapping.mutable_airdropboundary(), CENTER_LAT - 0.005, CENTER_LNG + 0.03); + addCoord(overlapping.mutable_airdropboundary(), CENTER_LAT + 0.005, CENTER_LNG + 0.03); + addCoord(overlapping.mutable_airdropboundary(), CENTER_LAT + 0.005, CENTER_LNG); + + EXPECT_TRUE(upload(overlapping).has_value()); +} + +/* + * MissionParameters::setMission -- every way the mission is wrong is reported at + * once, so the operator does not have to fix them one upload at a time + */ +TEST(MissionParametersTest, EveryProblemIsReportedTogether) { + Mission mission = validMission(); + addCoord(mission.mutable_waypoints(), CENTER_LAT + 0.02, CENTER_LNG); + mission.clear_airdropboundary(); + addSquare(mission.mutable_airdropboundary(), 0.02); + + const std::optional err = upload(mission); + + ASSERT_TRUE(err.has_value()); + EXPECT_NE(err->find("Waypoint"), std::string::npos) << err.value(); + EXPECT_NE(err->find("Airdrop boundary"), std::string::npos) << err.value(); +} diff --git a/tests/unit/pathing/dubins_test.cpp b/tests/unit/pathing/dubins_test.cpp index 95ef87ff..1960e5d3 100644 --- a/tests/unit/pathing/dubins_test.cpp +++ b/tests/unit/pathing/dubins_test.cpp @@ -2,7 +2,10 @@ #include +#include #include +#include +#include #include "utilities/datatypes.hpp" @@ -13,6 +16,68 @@ static inline void setDubins(double r, double sep) { Dubins::_point_separation = sep; } +namespace { + +// headings are only meaningful mod 2pi, so compare them on the circle +void expectHeadingNear(double actual, double expected, double tolerance) { + const double difference = std::abs(mod(actual - expected + M_PI, TWO_PI) - M_PI); + EXPECT_LT(difference, tolerance) << "heading " << actual << " is not " << expected; +} + +void expectPointNear(const Vector& actual, const Vector& expected, double tolerance) { + EXPECT_NEAR(actual.x, expected.x, tolerance); + EXPECT_NEAR(actual.y, expected.y, tolerance); +} + +/* + * The geometry every [LSL, LSR, RSR, RSL] point list has to satisfy: + * - it runs from the start vector to the end vector + * - every point sits on one of the two turning circles (the straightaway + * runs between the tangent points, which are on the circles themselves) + * - the turns are sampled at _point_separation, and the straightaway is + * described by its two endpoints alone, so that ardupilot accelerates + * through it instead of slowing down for every point along the way + */ +void expectStraightPathPoints(const RRTPoint& start, const RRTPoint& end, const DubinsPath& path, + const std::vector& points) { + const double radius = Dubins::_radius; + const Vector center_0 = Dubins::findCenter(start, (path.beta_0 > 0) ? 'L' : 'R'); + const Vector center_2 = Dubins::findCenter(end, (path.beta_2 > 0) ? 'L' : 'R'); + + ASSERT_GE(points.size(), 2); + expectPointNear(points.front(), start.coord, 1e-6); + expectPointNear(points.back(), end.coord, 1e-6); + + for (const Vector& point : points) { + const double off_first_turn = std::abs(point.distanceTo(center_0) - radius); + const double off_last_turn = std::abs(point.distanceTo(center_2) - radius); + EXPECT_LT(std::min(off_first_turn, off_last_turn), 1e-6) + << "(" << point.x << ", " << point.y << ") is on neither turning circle"; + } + + int straightaways = 0; + double flown = 0; + for (std::size_t i = 1; i < points.size(); i++) { + const double step = points[i].distanceTo(points[i - 1]); + flown += step; + + if (step > Dubins::_point_separation + 1e-6) { + straightaways++; + EXPECT_NEAR(step, path.straight_dist, 1e-6); + } + } + EXPECT_EQ(straightaways, (path.straight_dist > Dubins::_point_separation) ? 1 : 0); + + // the polyline cuts the corner off of every arc, so it comes out a little + // shorter than the path it approximates + const double length = + radius * (std::abs(path.beta_0) + std::abs(path.beta_2)) + path.straight_dist; + EXPECT_LT(flown, length + 1e-6); + EXPECT_GT(flown, length * 0.99); +} + +} // namespace + /* * NOTE: the use of () and {} constructors is non-staandard * i.e. I originally wrote it using () and was too lazy to @@ -211,7 +276,6 @@ TEST(DubinsTest, CircleArc) { /* * tests Dubins::generatePointsStraight() - * fails at last turn */ TEST(DubinsTest, GenPointsStraight) { setDubins(5, 1); @@ -220,84 +284,29 @@ TEST(DubinsTest, GenPointsStraight) { RRTPoint arbitrary_position1{Vector{9, 6, 0}, 4.00}; // lsl origin_x ==> arbitrary_position - DubinsPath path{6.107586558274035, 4.175598748905551, 12.983673916464376}; - - std::vector result1 = - 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}, - Vector{2.823212366975177, 0.8733219254516085, 0}, - Vector{3.5867804544976143, 1.5164664532641732, 0}, - Vector{4.207354924039483, 2.2984884706593016, 0}, - Vector{4.660195429836132, 3.188211227616632, 0}, - Vector{4.9272486499423005, 4.1501642854987955, 0}, - Vector{4.997868015207525, 5.145997611506444, 0}, - Vector{4.869238154390976, 6.136010473465436, 0}, - Vector{4.546487134128409, 7.080734182735712, 0}, - Vector{4.0424820190979505, 7.942505586276729, 0}, - Vector{3.3773159027557553, 8.686968577706228, 0}, - Vector{2.5775068591073205, 9.284443766844737, 0}, - Vector{1.6749407507795255, 9.71111170334329, 0}, - Vector{0.7056000402993361, 9.949962483002228, 0}, - Vector{-0.2918707171379004, 9.991473878973766, 0}, - Vector{-1.2777055101341561, 9.833990962897305, 0}, - Vector{-2.212602216474262, 9.483792081670735, 0}, - Vector{-3.059289454713595, 8.954838559572083, 0}, - Vector{-3.7840124765396412, 8.26821810431806, 0}, - Vector{-4.357878862067941, 7.451304106703497, 0}, - Vector{-4.758010369447581, 6.536664349892097, 0}, - Vector{-4.968455018167322, 5.5607626346752745, 0}, - Vector{-4.980823044179203, 4.562505082802768, 0}, - Vector{-4.794621373315692, 3.5816890726838686, 0}, - Vector{-4.417273278600765, 2.6574166434981144, 0}, - Vector{-3.8638224377799353, 1.8265356202868266, 0}, - Vector{-3.156333189361607, 1.1221706074487519, 0}, - Vector{-2.323010897068786, 0.5724024152934053, 0}, - Vector{-1.3970774909946289, 0.1991485667481694, 0}, - Vector{-0.4185269590432342, -0.0038326600561875424, 0}, - Vector{0.5660951562987457, -0.1785303703430865, 0}, - Vector{1.5507172716407256, -0.35322808062998534, 0}, - Vector{2.535339386982705, -0.5279257909168843, 0}, - Vector{3.519961502324685, -0.7026235012037831, 0}, - Vector{4.504583617666665, -0.8773212114906821, 0}, - Vector{5.489205733008644, -1.052018921777581, 0}, - Vector{6.473827848350624, -1.22671663206448, 0}, - Vector{7.458449963692604, -1.401414342351379, 0}, - Vector{8.443072079034584, -1.5761120526382777, 0}, - Vector{9.427694194376564, -1.7508097629251764, 0}, - Vector{10.412316309718543, -1.9255074732120754, 0}, - Vector{11.396938425060522, -2.1002051834989746, 0}, - Vector{12.384837277736102, -2.2522585496455405, 0}, - Vector{13.382970185417884, -2.2322134370087867, 0}, - Vector{14.357224539367046, -2.014269495250773, 0}, - Vector{15.26875989292903, -1.6071154615675085, 0}, - Vector{16.081236208073463, -1.0269832824330973, 0}, - Vector{16.762262618032818, -0.2970009971166694, 0}, - Vector{17.284688748945122, 0.5537293042585145, 0}, - Vector{17.6276871195819, 1.491291689211344, 0}, - Vector{17.777583467299806, 2.478308504102941, 0}, - Vector{17.728401897740607, 3.4754305032352506, 0}, - Vector{17.482103124881213, 4.442905579103292, 0}, - Vector{17.048506303554806, 5.342163553476361, 0}, - Vector{16.444897570733772, 6.1373538486888375, 0}, - Vector{15.695340901798186, 6.796774737038687, 0}, - Vector{14.829718755775655, 7.294137188630348, 0}, - Vector{13.882540755999456, 7.609612932036834, 0}, - Vector{12.89156790032764, 7.73062494490981, 0}, - Vector{11.896307149320169, 7.652348860171518, 0}, - Vector{10.936436408392781, 7.377905298306002, 0}, - Vector{10.050222694938975, 6.918235458068104, 0}, - Vector{9.272996553105063, 6.2916649254160415, 0}, - Vector{9.0, 6.0, 0}}; - - EXPECT_EQ(result1.size(), expected_result1.size()); - - // could throw an indexOutOfBounds error - for (int i = 0; i < result1.size(); i++) { - EXPECT_NEAR(result1[i].x, expected_result1[i].x, 0.01); - EXPECT_NEAR(result1[i].y, expected_result1[i].y, 0.01); - } + DubinsPath lsl{6.107586558274035, 4.175598748905551, 12.983673916464376}; + expectStraightPathPoints(origin_x, arbitrary_position1, lsl, + Dubins::generatePointsStraight(origin_x, arbitrary_position1, lsl)); + + // rsr origin_x ==> arbitrary_position, the same two vectors turning the other way + DubinsPath rsr{-5.062863952455051, -3.5035066619041215, 15.191727147276039}; + expectStraightPathPoints(origin_x, arbitrary_position1, rsr, + Dubins::generatePointsStraight(origin_x, arbitrary_position1, rsr)); + + // rsl origin_x ==> arbitrary_position, one turn each way + DubinsPath rsl{-0.18936765807467593, 4.189367658074676, 11.100064246783269}; + expectStraightPathPoints(origin_x, arbitrary_position1, rsl, + Dubins::generatePointsStraight(origin_x, arbitrary_position1, rsl)); + + // a path that never turns is nothing but the straightaway, so it is described + // by its two endpoints + RRTPoint straight_end{Vector{20, 0, 0}, 0}; + std::vector straight = + Dubins::generatePointsStraight(origin_x, straight_end, DubinsPath(0, 0, 20)); + + ASSERT_EQ(straight.size(), 2); + expectPointNear(straight[0], origin_x.coord, 1e-6); + expectPointNear(straight[1], straight_end.coord, 1e-6); } /* @@ -364,6 +373,8 @@ TEST(DubinsTest, GenPointsCurve) { /* * tests Dubins::generatePoints() + * + * generatePoints only picks which of the two generators to hand the path to */ TEST(DubinsTest, GenPoints) { setDubins(5, 1); @@ -378,131 +389,35 @@ TEST(DubinsTest, GenPoints) { RRTOption lrl{37.28571149387029, DubinsPath{2.25948315258286, 0.3274953432143759, -4.870163802976823}, false}; - std::vector result1 = - Dubins::generatePoints(origin_x, arbitrary_position1, lsl.dubins_path, lsl.has_straight); - std::vector result2 = - 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}, - Vector{1.9470917115432524, 0.3946950299855745, 0}, - Vector{2.823212366975177, 0.8733219254516085, 0}, - Vector{3.5867804544976143, 1.5164664532641732, 0}, - Vector{4.207354924039483, 2.2984884706593016, 0}, - Vector{4.660195429836132, 3.188211227616632, 0}, - Vector{4.9272486499423005, 4.1501642854987955, 0}, - Vector{4.997868015207525, 5.145997611506444, 0}, - Vector{4.869238154390976, 6.136010473465436, 0}, - Vector{4.546487134128409, 7.080734182735712, 0}, - Vector{4.0424820190979505, 7.942505586276729, 0}, - Vector{3.3773159027557553, 8.686968577706228, 0}, - Vector{2.5775068591073205, 9.284443766844737, 0}, - Vector{1.6749407507795255, 9.71111170334329, 0}, - Vector{0.7056000402993361, 9.949962483002228, 0}, - Vector{-0.2918707171379004, 9.991473878973766, 0}, - Vector{-1.2777055101341561, 9.833990962897305, 0}, - Vector{-2.212602216474262, 9.483792081670735, 0}, - Vector{-3.059289454713595, 8.954838559572083, 0}, - Vector{-3.7840124765396412, 8.26821810431806, 0}, - Vector{-4.357878862067941, 7.451304106703497, 0}, - Vector{-4.758010369447581, 6.536664349892097, 0}, - Vector{-4.968455018167322, 5.5607626346752745, 0}, - Vector{-4.980823044179203, 4.562505082802768, 0}, - Vector{-4.794621373315692, 3.5816890726838686, 0}, - Vector{-4.417273278600765, 2.6574166434981144, 0}, - Vector{-3.8638224377799353, 1.8265356202868266, 0}, - Vector{-3.156333189361607, 1.1221706074487519, 0}, - Vector{-2.323010897068786, 0.5724024152934053, 0}, - Vector{-1.3970774909946289, 0.1991485667481694, 0}, - Vector{-0.4185269590432342, -0.0038326600561875424, 0}, - Vector{0.5660951562987457, -0.1785303703430865, 0}, - Vector{1.5507172716407256, -0.35322808062998534, 0}, - Vector{2.535339386982705, -0.5279257909168843, 0}, - Vector{3.519961502324685, -0.7026235012037831, 0}, - Vector{4.504583617666665, -0.8773212114906821, 0}, - Vector{5.489205733008644, -1.052018921777581, 0}, - Vector{6.473827848350624, -1.22671663206448, 0}, - Vector{7.458449963692604, -1.401414342351379, 0}, - Vector{8.443072079034584, -1.5761120526382777, 0}, - Vector{9.427694194376564, -1.7508097629251764, 0}, - Vector{10.412316309718543, -1.9255074732120754, 0}, - Vector{11.396938425060522, -2.1002051834989746, 0}, - Vector{12.384837277736102, -2.2522585496455405, 0}, - Vector{13.382970185417884, -2.2322134370087867, 0}, - Vector{14.357224539367046, -2.014269495250773, 0}, - Vector{15.26875989292903, -1.6071154615675085, 0}, - Vector{16.081236208073463, -1.0269832824330973, 0}, - Vector{16.762262618032818, -0.2970009971166694, 0}, - Vector{17.284688748945122, 0.5537293042585145, 0}, - Vector{17.6276871195819, 1.491291689211344, 0}, - Vector{17.777583467299806, 2.478308504102941, 0}, - Vector{17.728401897740607, 3.4754305032352506, 0}, - Vector{17.482103124881213, 4.442905579103292, 0}, - Vector{17.048506303554806, 5.342163553476361, 0}, - Vector{16.444897570733772, 6.1373538486888375, 0}, - Vector{15.695340901798186, 6.796774737038687, 0}, - Vector{14.829718755775655, 7.294137188630348, 0}, - Vector{13.882540755999456, 7.609612932036834, 0}, - Vector{12.89156790032764, 7.73062494490981, 0}, - Vector{11.896307149320169, 7.652348860171518, 0}, - Vector{10.936436408392781, 7.377905298306002, 0}, - Vector{10.050222694938975, 6.918235458068104, 0}, - Vector{9.272996553105063, 6.2916649254160415, 0}, - Vector{9.0, 6.0, 0}}; + const std::vector straight = Dubins::generatePoints( + origin_x, arbitrary_position1, lsl.dubins_path, lsl.has_straight); + const std::vector expected_straight = + Dubins::generatePointsStraight(origin_x, arbitrary_position1, lsl.dubins_path); - std::vector expected_result2 = {Vector{6.123233995736766e-16, 0.0, 0}, - Vector{0.9933466539753065, 0.09966711079379209, 0}, - Vector{1.9470917115432524, 0.3946950299855745, 0}, - Vector{2.823212366975177, 0.8733219254516085, 0}, - Vector{3.5867804544976143, 1.5164664532641732, 0}, - Vector{4.207354924039483, 2.2984884706593016, 0}, - Vector{4.660195429836132, 3.188211227616632, 0}, - Vector{4.9272486499423005, 4.1501642854987955, 0}, - Vector{4.997868015207525, 5.145997611506444, 0}, - Vector{4.869238154390976, 6.136010473465436, 0}, - Vector{4.546487134128409, 7.080734182735712, 0}, - Vector{4.0424820190979505, 7.942505586276729, 0}, - Vector{3.453414225327183, 8.749607451779864, 0}, - Vector{3.0208190902513206, 9.649347723773186, 0}, - Vector{2.7755978543530304, 10.617096479082347, 0}, - Vector{2.727526714467218, 11.61427262874097, 0}, - Vector{2.878522115243877, 12.601121906433475, 0}, - Vector{3.222564346547582, 13.538301745641235, 0}, - Vector{3.7459375303042997, 14.388449743617045, 0}, - Vector{4.427776429277235, 15.11767318247875, 0}, - Vector{5.240898278231409, 15.69690022491657, 0}, - Vector{6.1528864750033945, 16.10303891660514, 0}, - Vector{7.127382928133069, 16.31989778955159, 0}, - Vector{8.125537539235536, 16.338831364829296, 0}, - Vector{9.107557033825168, 16.15908482054028, 0}, - Vector{10.03429139359439, 15.787824084182331, 0}, - Vector{10.868794644098593, 15.239850149733034, 0}, - Vector{11.57779777416746, 14.537009008727138, 0}, - Vector{12.133035066393902, 13.707320719513074, 0}, - Vector{12.512370962089452, 12.783862335949, 0}, - Vector{12.700682536156371, 11.803449229646171, 0}, - Vector{12.690462400388142, 10.805167377280418, 0}, - Vector{12.48211799934634, 9.828815126045004, 0}, - Vector{12.08395536683173, 8.913316559139, 0}, - Vector{11.511847990527825, 8.095169715402147, 0}, - Vector{10.788603986138625, 7.4069915276825675, 0}, - Vector{9.955581925670863, 6.854885170996804, 0}, - Vector{9.192828057382332, 6.210775208064388, 0}, - Vector{9.0, 6.0, 0}}; - - EXPECT_EQ(result1.size(), expected_result1.size()); - EXPECT_EQ(result2.size(), expected_result2.size()); - - // could throw an indexOutOfBounds error - for (int i = 0; i < result1.size(); i++) { - EXPECT_NEAR(result1[i].x, expected_result1[i].x, 0.01); - EXPECT_NEAR(result1[i].y, expected_result1[i].y, 0.01); + ASSERT_EQ(straight.size(), expected_straight.size()); + for (std::size_t i = 0; i < straight.size(); i++) { + expectPointNear(straight[i], expected_straight[i], 1e-9); + } + expectStraightPathPoints(origin_x, arbitrary_position1, lsl.dubins_path, straight); + + const std::vector curve = Dubins::generatePoints( + origin_x, arbitrary_position1, lrl.dubins_path, lrl.has_straight); + // generatePointsCurve reads the middle turn out of straight_dist, and wants it + // as a magnitude + const std::vector expected_curve = Dubins::generatePointsCurve( + origin_x, arbitrary_position1, lrl.dubins_path); + + ASSERT_EQ(curve.size(), expected_curve.size()); + for (std::size_t i = 0; i < curve.size(); i++) { + expectPointNear(curve[i], expected_curve[i], 1e-9); } - // could throw an indexOutOfBounds error - for (int i = 0; i < result2.size(); i++) { - EXPECT_NEAR(result2[i].x, expected_result2[i].x, 0.01); - EXPECT_NEAR(result2[i].y, expected_result2[i].y, 0.01); + // a path that is all turns is sampled the whole way -- there is no straightaway + // to skip over + expectPointNear(curve.front(), origin_x.coord, 1e-6); + expectPointNear(curve.back(), arbitrary_position1.coord, 1e-6); + for (std::size_t i = 1; i < curve.size(); i++) { + EXPECT_LT(curve[i].distanceTo(curve[i - 1]), Dubins::_point_separation + 1e-6); } } @@ -821,46 +736,123 @@ TEST(DubinsTest, DubinsPath) { RRTPoint origin_x{Vector{0, 0, 0}, 0}; RRTPoint arbitrary_position1{Vector{9, 6, 0}, 4.00}; - 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}, - Vector{2.957647760721099, -0.4758655270451597, 0}, - Vector{3.939771222897386, -0.6641034218561235, 0}, - Vector{4.9218946850736724, -0.8523413166670872, 0}, - Vector{5.904018147249959, -1.040579211478051, 0}, - Vector{6.886141609426246, -1.2288171062890147, 0}, - Vector{7.868265071602533, -1.4170550010999785, 0}, - Vector{8.850388533778819, -1.6052928959109423, 0}, - Vector{9.832511995955107, -1.793530790721906, 0}, - Vector{10.814635458131391, -1.9817686855328698, 0}, - Vector{11.79675892030768, -2.1700065803438333, 0}, - Vector{12.79027164737483, -2.26821418659457, 0}, - Vector{13.783492756324007, -2.1673036486132564, 0}, - Vector{14.736867740560596, -1.8710820299393291, 0}, - Vector{15.61238854801182, -1.3913587517279438, 0}, - Vector{16.375150926950273, -0.7472588672270439, 0}, - Vector{16.994745948363555, 0.035539393743043934, 0}, - Vector{17.446472313993077, 0.9258283347801974, 0}, - Vector{17.712321119146168, 1.8881149452744896, 0}, - Vector{17.781693810895792, 2.8840358947365097, 0}, - Vector{17.65182471894008, 3.873886957721502, 0}, - Vector{17.32789131414969, 4.818205896003914, 0}, - Vector{16.822807799142637, 5.67934569348516, 0}, - Vector{16.15671025977906, 6.422975423923436, 0}, - Vector{15.356153902961042, 7.019448916625484, 0}, - Vector{14.453054384333244, 7.444986655718225, 0}, - Vector{13.48341543180803, 7.68262379440602, 0}, - Vector{12.48589349054992, 7.722886489876743, 0}, - Vector{11.500256612494358, 7.56416959551826, 0}, - Vector{10.565799029612386, 7.212800653048518, 0}, - Vector{9.719774616882088, 6.682787633394971, 0}, - Vector{9.0, 6.0, 0}}; + // the path flown is the shortest option there is + const RRTOption best = Dubins::bestOption(origin_x, arbitrary_position1); + for (const RRTOption& option : Dubins::allOptions(origin_x, arbitrary_position1)) { + EXPECT_LE(best.length, option.length); + } - EXPECT_EQ(result1.size(), expected_result1.size()); + const std::vector result = Dubins::dubinsPath(origin_x, arbitrary_position1); + const std::vector expected = Dubins::generatePoints( + origin_x, arbitrary_position1, best.dubins_path, best.has_straight); - for (int i = 0; i < result1.size(); i++) { - EXPECT_NEAR(result1[i].x, expected_result1[i].x, 0.01); - EXPECT_NEAR(result1[i].y, expected_result1[i].y, 0.01); + ASSERT_EQ(result.size(), expected.size()); + for (std::size_t i = 0; i < result.size(); i++) { + expectPointNear(result[i], expected[i], 1e-9); + } + + ASSERT_TRUE(best.has_straight); + expectStraightPathPoints(origin_x, arbitrary_position1, best.dubins_path, result); +} + +/* + * ============================================================================ + * Dubins::generatePath + * + * A sequence of dubins paths, each one flown from the vector the previous one + * ended on, stitched into a single list of points. + * ============================================================================ + */ + +/* + * tests Dubins::generatePath() -- nothing to fly + */ +TEST(DubinsTest, GeneratePathWithNoSegments) { + setDubins(5, 1); + + EXPECT_TRUE(Dubins::generatePath(RRTPoint{Vector{0, 0, 0}, 0}, {}).empty()); +} + +/* + * tests Dubins::generatePath() -- a single segment, which is the points of that + * segment without the point the plane is already sitting on + */ +TEST(DubinsTest, GeneratePathWithOneSegment) { + setDubins(5, 1); + + const RRTPoint start{Vector{0, 0, 0}, 0}; + const RRTPoint end{Vector{9, 6, 0}, 4.00}; + const RRTOption option = Dubins::bestOption(start, end); + + const std::vector expected = + Dubins::generatePoints(start, end, option.dubins_path, option.has_straight); + const std::vector path = Dubins::generatePath(start, {PathSegment(end, option)}); + + ASSERT_EQ(path.size(), expected.size() - 1); + for (std::size_t i = 0; i < path.size(); i++) { + expectPointNear(path[i], expected[i + 1], 1e-6); } -} \ No newline at end of file + + // the plane's own position is not repeated, and the path lands on the end + EXPECT_GT(path.front().distanceTo(start.coord), 0); + expectPointNear(path.back(), end.coord, 1e-6); +} + +/* + * tests Dubins::generatePath() -- segments are flown back to back, each one + * starting where the last one ended + */ +TEST(DubinsTest, GeneratePathChainsSegments) { + setDubins(5, 1); + + const RRTPoint start{Vector{0, 0, 0}, 0}; + const std::vector waypoints = { + RRTPoint{Vector{30, 10, 0}, HALF_PI}, + RRTPoint{Vector{10, 50, 0}, M_PI}, + RRTPoint{Vector{-30, 20, 0}, 3 * HALF_PI}, + }; + + std::vector segments; + RRTPoint current = start; + for (const RRTPoint& waypoint : waypoints) { + segments.emplace_back(waypoint, Dubins::bestOption(current, waypoint)); + current = waypoint; + } + + const std::vector path = Dubins::generatePath(start, segments); + ASSERT_FALSE(path.empty()); + + // the path visits every waypoint, in order, and ends on the last one + std::size_t index = 0; + for (const RRTPoint& waypoint : waypoints) { + bool found = false; + for (; index < path.size(); index++) { + if (path[index].distanceTo(waypoint.coord) < 1e-6) { + found = true; + break; + } + } + EXPECT_TRUE(found) << "path never reached (" << waypoint.coord.x << ", " + << waypoint.coord.y << ")"; + } + expectPointNear(path.back(), waypoints.back().coord, 1e-6); + + // the joints are not duplicated -- no two points in a row are identical + for (std::size_t i = 1; i < path.size(); i++) { + EXPECT_GT(path[i].distanceTo(path[i - 1]), 0) + << "duplicate point at index " << i; + } + + // the segments concatenate: dropping the first point of each one accounts for + // every point in the path + std::size_t expected_size = 0; + current = start; + for (const PathSegment& segment : segments) { + expected_size += Dubins::generatePoints(current, segment.end, segment.option.dubins_path, + segment.option.has_straight) + .size() - + 1; + current = segment.end; + } + EXPECT_EQ(path.size(), expected_size); +} diff --git a/tests/unit/pathing/rrt_test.cpp b/tests/unit/pathing/rrt_test.cpp new file mode 100644 index 00000000..e566e0a6 --- /dev/null +++ b/tests/unit/pathing/rrt_test.cpp @@ -0,0 +1,690 @@ +#include "pathing/rrt.hpp" + +#include + +#include +#include +#include +#include + +#include "pathing/dubins.hpp" +#include "pathing/environment.hpp" +#include "pathing/tree.hpp" +#include "utilities/constants.hpp" +#include "utilities/datatypes.hpp" +#include "utilities/rng.hpp" + +// the state rand_r() is walked from, so a test that samples can be repeated +extern unsigned int seed1; + +namespace { + +void seedRandom(unsigned int seed) { seed1 = seed; } + +static inline void setDubins(double r, double sep) { + Dubins::_radius = r; + Dubins::_point_separation = sep; +} + +// 1000 x 1000 field, nothing in it. A 30m turning radius leaves plenty of room +const Polygon FIELD = {{XYZCoord(0, 0, 0), XYZCoord(1000, 0, 0), XYZCoord(1000, 1000, 0), + XYZCoord(0, 1000, 0)}}; + +void initOpenField() { + Environment::init(FIELD, {}, {}, {}); + setDubins(30, 10); +} + +// a wall that splits the field at x in [480, 520], with a 300m gap at the top +const Polygon WALL = {{XYZCoord(480, 0, 0), XYZCoord(520, 0, 0), XYZCoord(520, 700, 0), + XYZCoord(480, 700, 0)}}; + +void initFieldWithWall() { + Environment::init(FIELD, {}, {}, {WALL}); + setDubins(30, 10); +} + +// the cheapest connection from anywhere in the tree to any of the given points +// that can actually be flown, found by brute force +Connection cheapestFlyableConnection(const RRT& rrt, const std::vector& ends) { + Connection best; + + for (NodeId node = 0; node < rrt.tree.tree.size; node++) { + const RRTPoint& anchor = rrt.tree.tree.points[node]; + + for (const RRTPoint& end : ends) { + for (const RRTOption& option : Dubins::allOptions(anchor, end)) { + const double cost = rrt.tree.tree.length[node] + option.length; + + if (std::isfinite(cost) && cost < best.cost && + Environment::isDubinsPathInBounds(anchor, end, option)) { + best = {node, end, option, cost}; + } + } + } + } + + return best; +} + +bool pathIsInBounds(const std::vector& path) { + for (const XYZCoord& point : path) { + if (!Environment::isPointInBounds(point)) { + return false; + } + } + return true; +} + +// the index of the first point of the path that lands on a waypoint, searching +// from `from` so that waypoints can be checked in the order they are flown +std::size_t indexOfPoint(const std::vector& path, const XYZCoord& target, + std::size_t from = 0) { + for (std::size_t i = from; i < path.size(); i++) { + if (std::hypot(path[i].x - target.x, path[i].y - target.y) < 1e-6) { + return i; + } + } + return path.size(); +} + +// the heading the path is flying as it lands on the point at `index` +double headingAt(const std::vector& path, std::size_t index) { + const XYZCoord& previous = path[index - 1]; + return std::atan2(path[index].y - previous.y, path[index].x - previous.x); +} + +// how far apart two headings are, the short way around +double angleBetween(double a, double b) { + return std::abs(std::remainder(a - b, TWO_PI)); +} + +} // namespace + +/* + * A fresh RRT holds nothing but the plane's current vector, which is the first + * of the points it flies through + */ +TEST(RRTTest, ConstructionSeedsTheTreeWithTheStart) { + initOpenField(); + const std::vector goals = {XYZCoord(100, 100, 0), XYZCoord(500, 500, 30)}; + + const RRT rrt(goals, HALF_PI); + + // the tree is rooted where the plane is, flying the heading it was given + EXPECT_TRUE(rrt.tree.getStart() == RRTPoint(goals[0], HALF_PI)); + EXPECT_EQ(rrt.tree.tree.size, 1); + EXPECT_TRUE(rrt.getPointsToGoal().empty()); + EXPECT_EQ(rrt.goals, goals); + + // every approach angle is tried at every goal unless the caller asks for a + // specific set, and the plane's own heading stands in for the goal it is on + ASSERT_EQ(rrt.goal_angles.size(), goals.size()); + EXPECT_EQ(rrt.goal_angles[0], std::vector({HALF_PI})); + EXPECT_EQ(rrt.goal_angles[1], DEFAULT_GOAL_ANGLES); + + const std::vector angles = {0.0, M_PI}; + const RRT custom_angles(goals, HALF_PI, angles); + EXPECT_EQ(custom_angles.goal_angles[1], angles); +} + +/* + * A caller that dictates how a goal is approached -- coverage pathing does, as a + * scan line has to be flown along its own direction -- leaves it a single angle + */ +TEST(RRTTest, ConstructionPinsTheGoalsTheCallerNamedAnAngleFor) { + initOpenField(); + const std::vector goals = {XYZCoord(100, 100, 0), XYZCoord(500, 500, 30), + XYZCoord(800, 200, 30)}; + const std::vector> goal_angles = {{}, {0}, {M_PI}}; + + const RRT rrt(goals, HALF_PI, goal_angles); + + EXPECT_TRUE(rrt.tree.getStart() == RRTPoint(goals[0], HALF_PI)); + EXPECT_EQ(rrt.tree.tree.size, 1); + EXPECT_EQ(rrt.goals, goals); + + // whatever the caller put down for the goal the plane is already sitting on, + // the heading it reached that one at is the one it is flying + EXPECT_EQ(rrt.goal_angles[0], std::vector({HALF_PI})); + EXPECT_EQ(rrt.goal_angles[1], std::vector({0})); + EXPECT_EQ(rrt.goal_angles[2], std::vector({M_PI})); +} + +/* + * RRT::goalEndpoints -- a goal left a single angle is only reachable the one way + */ +TEST(RRTTest, GoalEndpointsHonorAPinnedAngle) { + initOpenField(); + const std::vector goals = {XYZCoord(100, 100, 0), XYZCoord(500, 500, 30), + XYZCoord(800, 200, 30)}; + const std::vector> goal_angles = {{}, {HALF_PI}, {M_PI}}; + + const RRT rrt(goals, 0, goal_angles); + + // the plane sits on the first of them, it is flown from and never to + for (std::size_t goal = 1; goal < goals.size(); goal++) { + const std::vector ends = rrt.goalEndpoints(goal); + + ASSERT_EQ(ends.size(), 1); + EXPECT_TRUE(ends[0] == RRTPoint(goals[goal], goal_angles[goal][0])); + } +} + +/* + * RRT::run -- the way between the waypoints is up to RRT, but the heading a pinned + * one is reached at is not + */ +TEST(RRTTest, RunReachesEveryWaypointAtItsPinnedAngle) { + initOpenField(); + seedRandom(7); + + // scan lines, the way coverage pathing lays them out: swept one way, then back + const std::vector goals = {XYZCoord(100, 100, 0), XYZCoord(200, 300, 30), + XYZCoord(800, 300, 30), XYZCoord(800, 400, 30), + XYZCoord(200, 400, 30)}; + const std::vector> goal_angles = {{}, {0}, {0}, {M_PI}, {M_PI}}; + + RRT rrt(goals, 0, goal_angles); + rrt.run(); + + const std::vector path = rrt.getPointsToGoal(); + ASSERT_FALSE(path.empty()); + EXPECT_TRUE(pathIsInBounds(path)); + + std::size_t index = 0; + for (std::size_t goal = 1; goal < goals.size(); goal++) { + index = indexOfPoint(path, goals[goal], index); + ASSERT_LT(index, path.size()) << "path never reached waypoint " << goal; + ASSERT_GT(index, 0); + + // the points are far enough apart that the last leg of an arc only + // approximates the heading it lands on + EXPECT_LT(angleBetween(headingAt(path, index), goal_angles[goal][0]), 0.25) + << "waypoint " << goal << " was not flown at the angle it was pinned to"; + EXPECT_NEAR(path[index].z, goals[goal].z, 1e-9); + } + + EXPECT_EQ(indexOfPoint(path, goals.back(), index), path.size() - 1); +} + +/* + * RRT::generateDubinsOptions finds the paths and stops there -- what they cost is + * known without flying them, so a mission can be weighed against another one and + * thrown away without ever generating a point + */ +TEST(RRTTest, DubinsOptionsAreFoundWithoutFlyingThem) { + initOpenField(); + seedRandom(13); + const std::vector goals = {XYZCoord(100, 100, 0), XYZCoord(400, 300, 30), + XYZCoord(800, 600, 30)}; + + RRT rrt(goals, 0); + EXPECT_EQ(rrt.pathLength(), 0); + EXPECT_TRUE(rrt.getPointsToGoal().empty()); + + rrt.generateDubinsOptions(); + + // one leg per waypoint flown to, each landing on the goal it was found for + ASSERT_EQ(rrt.legs.size(), goals.size() - 1); + for (std::size_t i = 0; i < rrt.legs.size(); i++) { + EXPECT_EQ(rrt.legs[i].goal_idx, i + 1); + EXPECT_FALSE(rrt.legs[i].segments.empty()); + EXPECT_TRUE(rrt.legs[i].segments.back().end.coord == goals[i + 1]); + EXPECT_GT(rrt.legs[i].length, 0); + } + + // the legs start where the one behind them landed + EXPECT_TRUE(rrt.legs[0].start == RRTPoint(goals[0], 0)); + EXPECT_TRUE(rrt.legs[1].start == rrt.legs[0].segments.back().end); + + // how long the mission is is known, but not one point of it has been flown + double straight_line = 0; + for (std::size_t i = 1; i < goals.size(); i++) { + straight_line += std::hypot(goals[i].x - goals[i - 1].x, goals[i].y - goals[i - 1].y); + } + EXPECT_GE(rrt.pathLength(), straight_line); + EXPECT_TRUE(rrt.getPointsToGoal().empty()); + + rrt.generateFlightPoints(); + + const std::vector path = rrt.getPointsToGoal(); + ASSERT_FALSE(path.empty()); + + double flown = goals[0].distanceTo(path[0]); + for (std::size_t i = 1; i < path.size(); i++) { + flown += std::hypot(path[i].x - path[i - 1].x, path[i].y - path[i - 1].y); + } + + // the points cut the corners off the arcs, so they cover a little less ground + // than the legs they came from + EXPECT_LT(flown, rrt.pathLength()); + EXPECT_GT(flown, rrt.pathLength() * 0.95); + + // flying the legs a second time does not append the mission to itself + rrt.generateFlightPoints(); + EXPECT_EQ(rrt.getPointsToGoal().size(), path.size()); + + // and run() is the two of them, one after the other + RRT ran(goals, 0); + seedRandom(13); + ran.run(); + EXPECT_EQ(ran.legs.size(), rrt.legs.size()); + EXPECT_EQ(ran.getPointsToGoal().size(), path.size()); +} + +/* + * RRT::fillOptions -- every dubins path that exists from one node, and nothing else + */ +TEST(RRTTest, FillOptionsCollectsTheFlyablePathsFromANode) { + initOpenField(); + const RRTPoint start(XYZCoord(100, 100, 0), 0); + const RRT rrt({start.coord, XYZCoord(500, 500, 30)}, start.psi); + + const RRTPoint end(XYZCoord(500, 500, 0), HALF_PI); + rrt.fillOptions(0, {end}); + + // all four of the CSC paths exist between two vectors this far apart + EXPECT_EQ(rrt.options.size(), 4); + for (const Connection& option : rrt.options) { + EXPECT_EQ(option.anchor, 0); + EXPECT_TRUE(option.end == end); + EXPECT_TRUE(std::isfinite(option.option.length)); + + // the root has nothing behind it, so the flight is the path itself + EXPECT_DOUBLE_EQ(option.cost, option.option.length); + } + + // the scratch space is written over, not appended to + rrt.fillOptions(0, {end}); + EXPECT_EQ(rrt.options.size(), 4); + + // every endpoint asked for is pathed to + const RRTPoint other_end(XYZCoord(400, 600, 0), 0); + rrt.fillOptions(0, {end, other_end}); + EXPECT_EQ(rrt.options.size(), 8); + + // paths that do not exist are dropped -- the turning circles of these two + // vectors overlap, so there is no RSL path between them + rrt.fillOptions(0, {RRTPoint(XYZCoord(100, 80, 0), 0)}); + EXPECT_EQ(rrt.options.size(), 3); +} + +/* + * RRT::lowerBound -- a flight through a node never costs less than the ground it + * has to cover, which is what lets the search skip nodes it has not pathed from + */ +TEST(RRTTest, LowerBoundNeverExceedsWhatAFlightCosts) { + initOpenField(); + RRT rrt({XYZCoord(100, 100, 0), XYZCoord(900, 900, 30)}, 0); + + // a couple of branches, so the nodes sit at different distances from the root + const RRTPoint near_node(XYZCoord(200, 150, 0), 0); + const RRTPoint far_node(XYZCoord(700, 200, 0), HALF_PI); + rrt.tree.addSample(0, near_node, Dubins::bestOption(rrt.tree.getStart(), near_node)); + rrt.tree.addSample(1, far_node, Dubins::bestOption(near_node, far_node)); + ASSERT_EQ(rrt.tree.tree.size, 3); + + const std::vector ends = {RRTPoint(XYZCoord(800, 800, 0), HALF_PI), + RRTPoint(XYZCoord(300, 900, 0), 0)}; + + for (NodeId node = 0; node < rrt.tree.tree.size; node++) { + const double bound = rrt.lowerBound(node, ends); + + // the flight to the node itself is already paid for + EXPECT_GE(bound, rrt.tree.tree.length[node]); + + rrt.fillOptions(node, ends); + ASSERT_FALSE(rrt.options.empty()); + + for (const Connection& option : rrt.options) { + EXPECT_LE(bound, option.cost); + } + } +} + +/* + * RRT::bestConnection -- a path that leaves the airspace is not one that can be + * taken, no matter how cheap it is + */ +TEST(RRTTest, BestConnectionStaysInsideTheAirspace) { + initFieldWithWall(); + const RRTPoint start(XYZCoord(200, 400, 0), 0); + RRT rrt({start.coord, XYZCoord(900, 100, 30)}, start.psi); + + // the plane is boxed in against the wall, so the way to some of these is not + // the shortest one + const std::vector targets = {RRTPoint(XYZCoord(400, 200, 0), M_PI), + RRTPoint(XYZCoord(300, 800, 0), HALF_PI), + RRTPoint(XYZCoord(120, 400, 0), M_PI), + RRTPoint(XYZCoord(460, 650, 0), 0)}; + + for (const RRTPoint& end : targets) { + const Connection connection = rrt.bestConnection({end}); + + if (connection.isValid()) { + EXPECT_TRUE(Environment::isDubinsPathInBounds(rrt.tree.tree.points[connection.anchor], + connection.end, connection.option)); + } + + // everything cheaper than what it settled on cuts through the wall or the + // edge of the field + rrt.fillOptions(0, {end}); + ASSERT_FALSE(rrt.options.empty()); + + for (const Connection& option : rrt.options) { + if (option.cost < connection.cost) { + EXPECT_FALSE(Environment::isDubinsPathInBounds( + rrt.tree.tree.points[option.anchor], option.end, option.option)) + << "passed up a cheaper path to (" << end.coord.x << ", " << end.coord.y << ")"; + } + } + } +} + +/* + * RRT::bestConnection -- the cheapest flight to the point that can be flown, and + * it is not committed to the tree + */ +TEST(RRTTest, BestConnectionIsTheCheapestOneThatCanBeFlown) { + initOpenField(); + RRT rrt({XYZCoord(100, 100, 0), XYZCoord(900, 900, 30)}, 0); + + // a couple of branches, so the nodes sit at different distances from the root + const RRTPoint near_node(XYZCoord(200, 150, 0), 0); + const RRTPoint far_node(XYZCoord(700, 200, 0), HALF_PI); + rrt.tree.addSample(0, near_node, Dubins::bestOption(rrt.tree.getStart(), near_node)); + rrt.tree.addSample(1, far_node, Dubins::bestOption(near_node, far_node)); + ASSERT_EQ(rrt.tree.tree.size, 3); + + const std::vector ends = {RRTPoint(XYZCoord(800, 800, 0), HALF_PI)}; + const Connection connection = rrt.bestConnection(ends); + const Connection cheapest = cheapestFlyableConnection(rrt, ends); + + ASSERT_TRUE(connection.isValid()); + EXPECT_EQ(connection.anchor, cheapest.anchor); + EXPECT_DOUBLE_EQ(connection.cost, cheapest.cost); + EXPECT_TRUE(connection.end == ends[0]); + EXPECT_DOUBLE_EQ(connection.cost, + rrt.tree.tree.length[connection.anchor] + connection.option.length); + + // the tree is left alone -- the caller decides whether to commit + EXPECT_EQ(rrt.tree.tree.size, 3); +} + +/* + * RRT::bestConnection -- any of the points will do, and the cheapest one wins + */ +TEST(RRTTest, BestConnectionTakesTheCheapestOfTheEndpoints) { + initOpenField(); + RRT rrt({XYZCoord(100, 100, 0), XYZCoord(900, 900, 30)}, 0); + + // straight ahead of the plane, and well off to the side of it + const RRTPoint close(XYZCoord(300, 100, 0), 0); + const RRTPoint distant(XYZCoord(800, 700, 0), M_PI); + + const Connection connection = rrt.bestConnection({distant, close}); + + ASSERT_TRUE(connection.isValid()); + EXPECT_TRUE(connection.end == close); + EXPECT_DOUBLE_EQ(connection.cost, cheapestFlyableConnection(rrt, {distant, close}).cost); +} + +/* + * RRT::bestConnection -- an unreachable point hands back an invalid connection + */ +TEST(RRTTest, BestConnectionGivesUpOnAnUnreachablePoint) { + initOpenField(); + RRT rrt({XYZCoord(100, 100, 0), XYZCoord(900, 900, 30)}, 0); + + const Connection connection = rrt.bestConnection({RRTPoint(XYZCoord(2000, 2000, 0), 0)}); + + EXPECT_FALSE(connection.isValid()); + EXPECT_EQ(connection.anchor, INVALID_NODE); + EXPECT_FALSE(std::isfinite(connection.cost)); +} + +/* + * RRT::bestConnection -- a point the wall stands in front of is given up on, while + * one on the same side of it as the plane is still found + */ +TEST(RRTTest, BestConnectionGivesUpOnAPointBehindAnObstacle) { + initFieldWithWall(); + RRT rrt({XYZCoord(200, 400, 0), XYZCoord(900, 100, 30)}, 0); + + // a single node cannot reach around the wall -- the gap is 300m above it, and + // every path that lands on this point comes in through the wall + const RRTPoint across(XYZCoord(800, 400, 0), 0); + EXPECT_FALSE(rrt.bestConnection({across}).isValid()); + + // the same tree still finds a point the wall is not in front of + const RRTPoint reachable(XYZCoord(400, 200, 0), M_PI); + EXPECT_TRUE(rrt.bestConnection({reachable}).isValid()); + + // and a point behind the wall does not stop the reachable one from winning + const Connection connection = rrt.bestConnection({across, reachable}); + ASSERT_TRUE(connection.isValid()); + EXPECT_TRUE(connection.end == reachable); +} + +/* + * RRT::goalEndpoints -- the goal is tried at every approach angle + */ +TEST(RRTTest, GoalEndpointsCoverEveryApproachAngle) { + initOpenField(); + const std::vector angles = {0.0, HALF_PI, M_PI}; + const std::vector goals = {XYZCoord(100, 100, 0), XYZCoord(500, 500, 30), + XYZCoord(800, 200, 45)}; + RRT rrt(goals, 0, angles); + + // the plane sits on the first of them, it is flown from and never to + for (std::size_t goal = 1; goal < goals.size(); goal++) { + const std::vector ends = rrt.goalEndpoints(goal); + + ASSERT_EQ(ends.size(), angles.size()); + for (std::size_t i = 0; i < ends.size(); i++) { + EXPECT_TRUE(ends[i].coord == goals[goal]); + EXPECT_DOUBLE_EQ(ends[i].psi, angles[i]); + } + } +} + +/* + * RRT::connectToGoal -- the flight path is extended and the tree restarts at the + * waypoint that was just reached + */ +TEST(RRTTest, ConnectToGoalCommitsThePathAndRestartsTheTree) { + initOpenField(); + const XYZCoord goal(500, 500, 30); + RRT rrt({XYZCoord(100, 100, 0), goal}, 0); + + ASSERT_TRUE(rrt.connectToGoal(1)); + rrt.generateFlightPoints(); + + const std::vector path = rrt.getPointsToGoal(); + ASSERT_FALSE(path.empty()); + EXPECT_TRUE(pathIsInBounds(path)); + EXPECT_NEAR(path.back().x, goal.x, 1e-6); + EXPECT_NEAR(path.back().y, goal.y, 1e-6); + + // the waypoint is the root of the tree the next waypoint is pathed from + EXPECT_EQ(rrt.tree.tree.size, 1); + EXPECT_TRUE(rrt.tree.getStart().coord == goal); + EXPECT_NE(std::find(rrt.goal_angles[1].begin(), rrt.goal_angles[1].end(), + rrt.tree.getStart().psi), + rrt.goal_angles[1].end()); +} + +/* + * RRT::connectToGoal -- a goal outside the airspace is never connected to + */ +TEST(RRTTest, ConnectToGoalFailsOnAnUnreachableGoal) { + initOpenField(); + const RRTPoint start(XYZCoord(100, 100, 0), 0); + RRT rrt({start.coord, XYZCoord(2000, 2000, 30)}, start.psi); + + EXPECT_FALSE(rrt.connectToGoal(1)); + EXPECT_TRUE(rrt.getPointsToGoal().empty()); + + // the tree is left as it was, still rooted at the plane + EXPECT_EQ(rrt.tree.tree.size, 1); + EXPECT_TRUE(rrt.tree.getStart() == start); +} + +/* + * RRT::commitConnection -- the plane climbs at a constant rate along the ground it + * covers, so the altitude is interpolated over the length of the segment + */ +TEST(RRTTest, CommitConnectionClimbsToTheWaypointAltitude) { + initOpenField(); + const std::vector goals = {XYZCoord(100, 100, 0), XYZCoord(500, 500, 100), + XYZCoord(800, 200, 50)}; + RRT rrt(goals, 0); + + // first leg: climbs from the plane's altitude (0) up to 100 + const RRTPoint first_goal(goals[1], 0); + rrt.commitConnection({0, first_goal, Dubins::bestOption(rrt.tree.getStart(), first_goal), 0}, + 1); + rrt.generateFlightPoints(); + + const std::vector first_leg = rrt.getPointsToGoal(); + ASSERT_FALSE(first_leg.empty()); + EXPECT_NEAR(first_leg.back().z, 100, 1e-9); + EXPECT_GT(first_leg.front().z, 0); + EXPECT_LT(first_leg.front().z, 100); + for (std::size_t i = 1; i < first_leg.size(); i++) { + EXPECT_GE(first_leg[i].z, first_leg[i - 1].z); + } + + // second leg: descends from the altitude of the waypoint behind it, not from + // the altitude the plane started the mission at + const RRTPoint second_goal(goals[2], 0); + rrt.commitConnection({0, second_goal, Dubins::bestOption(rrt.tree.getStart(), second_goal), 0}, + 2); + rrt.generateFlightPoints(); + + const std::vector path = rrt.getPointsToGoal(); + ASSERT_GT(path.size(), first_leg.size()); + EXPECT_NEAR(path.back().z, 50, 1e-9); + + for (std::size_t i = first_leg.size(); i < path.size(); i++) { + EXPECT_LE(path[i].z, 100); + EXPECT_GE(path[i].z, 50); + EXPECT_LE(path[i].z, path[i - 1].z); + } +} + +/* + * RRT::RRTIteration -- the goal is connected to once the sampling is done + */ +TEST(RRTTest, RRTIterationConnectsToAReachableGoal) { + initOpenField(); + seedRandom(11); + const XYZCoord goal(700, 700, 30); + RRT rrt({XYZCoord(100, 100, 0), goal}, 0); + + EXPECT_TRUE(rrt.RRTIteration(1)); + rrt.generateFlightPoints(); + + const std::vector path = rrt.getPointsToGoal(); + ASSERT_FALSE(path.empty()); + EXPECT_TRUE(pathIsInBounds(path)); + EXPECT_TRUE(rrt.tree.getStart().coord == goal); +} + +/* + * RRT::run -- every waypoint is flown, in order + */ +TEST(RRTTest, RunFliesEveryWaypointInOrder) { + initOpenField(); + seedRandom(3); + const std::vector goals = {XYZCoord(100, 100, 0), XYZCoord(400, 300, 30), + XYZCoord(800, 600, 45), XYZCoord(200, 800, 60)}; + RRT rrt(goals, 0); + + rrt.run(); + + const std::vector path = rrt.getPointsToGoal(); + ASSERT_FALSE(path.empty()); + EXPECT_TRUE(pathIsInBounds(path)); + + // the plane is already on the first of them, the path is what it flies after + std::size_t index = 0; + for (std::size_t goal = 1; goal < goals.size(); goal++) { + index = indexOfPoint(path, goals[goal], index); + ASSERT_LT(index, path.size()) + << "path never reached (" << goals[goal].x << ", " << goals[goal].y << ")"; + EXPECT_NEAR(path[index].z, goals[goal].z, 1e-9); + } + + // the path ends on the last waypoint, and so does the tree + EXPECT_EQ(indexOfPoint(path, goals.back(), index), path.size() - 1); + EXPECT_TRUE(rrt.tree.getStart().coord == goals.back()); +} + +/* + * RRT::run -- the path found around an obstacle stays in bounds the whole way + */ +TEST(RRTTest, RunPathsAroundAnObstacle) { + initFieldWithWall(); + seedRandom(23); + const XYZCoord goal(800, 400, 30); + RRT rrt({XYZCoord(200, 400, 0), goal}, 0); + + rrt.run(); + + const std::vector path = rrt.getPointsToGoal(); + ASSERT_FALSE(path.empty()) << "never made it around the wall"; + EXPECT_TRUE(pathIsInBounds(path)); + + // it got to the other side, and the only way across is the gap above the wall + EXPECT_LT(indexOfPoint(path, goal), path.size()); + + // a straightaway is described by its two endpoints alone, so checking the + // points is not enough -- no leg of the path may cut through the wall + for (std::size_t i = 1; i < path.size(); i++) { + EXPECT_FALSE(Environment::doesLineIntersectPolygon(path[i - 1], path[i], WALL)) + << "leg " << i << " cuts through the wall"; + } +} + +/* + * RRT::bestConnection -- the pruning is what makes the search cheap, so whatever + * the tree looks like and wherever the goal is, it has to come back with the same + * connection an exhaustive check would + */ +TEST(RRTTest, BestConnectionMatchesAnExhaustiveSearch) { + initFieldWithWall(); + seedRandom(101); + + const XYZCoord start(200, 400, 0); + + for (int trial = 0; trial < 25; trial++) { + // the goal is on the far side of the wall half of the time, so the search + // is made to give up as well as to succeed + const XYZCoord goal = (trial % 2 == 0) ? XYZCoord(300, 800, 30) : XYZCoord(900, 100, 30); + RRT rrt({start, goal}, 0); + + // a tree of random samples, grown the way an iteration would grow it + for (int i = 0; i < 40; i++) { + const RRTPoint sample(Environment::getRandomPoint(false, start), random(0, TWO_PI)); + const Connection connection = rrt.bestConnection({sample}); + + if (connection.isValid()) { + rrt.tree.addSample(connection.anchor, connection.end, connection.option); + } + } + ASSERT_GT(rrt.tree.tree.size, 1) << "trial " << trial << " grew nothing to search"; + + const std::vector ends = rrt.goalEndpoints(1); + const Connection found = rrt.bestConnection(ends); + const Connection exhaustive = cheapestFlyableConnection(rrt, ends); + + ASSERT_EQ(found.isValid(), exhaustive.isValid()) << "trial " << trial; + + if (found.isValid()) { + EXPECT_DOUBLE_EQ(found.cost, exhaustive.cost) << "trial " << trial; + EXPECT_EQ(found.anchor, exhaustive.anchor) << "trial " << trial; + } + } +} diff --git a/tests/unit/pathing/tree_test.cpp b/tests/unit/pathing/tree_test.cpp index 018a4b27..09fac980 100644 --- a/tests/unit/pathing/tree_test.cpp +++ b/tests/unit/pathing/tree_test.cpp @@ -2,93 +2,302 @@ #include +#include + #include "pathing/dubins.hpp" -#include "pathing/environment.hpp" #include "utilities/constants.hpp" #include "utilities/datatypes.hpp" -/* - * very bad tests, was too lazy to check if every parameter was correct, aka didn't bother to find - * the hardcoded values for the expected values. - */ -#include +namespace { static inline void setDubins(double r, double sep) { Dubins::_radius = r; Dubins::_point_separation = sep; } -TEST(SimpleTreeTest, addNodeTest) { - setDubins(5, 0.1); - Polygon valid_region; - valid_region.emplace_back(XYZCoord(0, 0, 0)); - valid_region.emplace_back(XYZCoord(100, 0, 0)); - valid_region.emplace_back(XYZCoord(100, 100, 0)); - valid_region.emplace_back(XYZCoord(0, 100, 0)); +// the tree only ever reads a dubins option's length, the rest is carried untouched +RRTOption option(double length) { return RRTOption(length, DubinsPath(0, 0, length), true); } + +RRTPoint point(double x, double y) { return RRTPoint(XYZCoord(x, y, 0), 0); } + +// the children of a node, in the order the sibling list holds them +std::vector childrenOf(const RRTTree& tree, NodeId parent) { + std::vector children; + for (NodeId child = tree.tree.first_child[parent]; child != INVALID_NODE; + child = tree.tree.next_sibling[child]) { + children.push_back(child); + } + return children; +} + +std::vector lengthsOf(const std::vector& segments) { + std::vector lengths; + for (const PathSegment& segment : segments) { + lengths.push_back(segment.option.length); + } + return lengths; +} + +} // namespace + +/* + * The root is the only node a fresh tree has, and it is node 0 + */ +TEST(TreeTest, RootIsTheOnlyNodeAtConstruction) { + const RRTPoint root_point = point(25, 25); + RRTTree tree(root_point); + + EXPECT_EQ(tree.tree.size, 1); + EXPECT_TRUE(tree.getStart() == root_point); + EXPECT_TRUE(tree.tree.points[0] == root_point); + + EXPECT_EQ(tree.tree.parent[0], INVALID_NODE); + EXPECT_EQ(tree.tree.first_child[0], INVALID_NODE); + EXPECT_EQ(tree.tree.next_sibling[0], INVALID_NODE); + EXPECT_EQ(tree.tree.length[0], 0.0); + + // the root's option is a no-op, the path starts where the root is + EXPECT_EQ(tree.tree.rrt_options[0].length, 0.0); +} + +/* + * The tree is fixed size, and big enough to hold every iteration RRT will run + */ +TEST(TreeTest, CapacityCoversEveryIteration) { + RRTTree tree(point(0, 0)); + + // the root, every sample of an iteration, and the goal they connect to + EXPECT_EQ(ITERATIONS_PER_WAYPOINT, TREE_CAPACITY - 2); + + EXPECT_EQ(tree.tree.points.size(), TREE_CAPACITY); + EXPECT_EQ(tree.tree.rrt_options.size(), TREE_CAPACITY); + EXPECT_EQ(tree.tree.length.size(), TREE_CAPACITY); + EXPECT_EQ(tree.tree.parent.size(), TREE_CAPACITY); + EXPECT_EQ(tree.tree.first_child.size(), TREE_CAPACITY); + EXPECT_EQ(tree.tree.next_sibling.size(), TREE_CAPACITY); +} + +/* + * A sample hangs off its parent, and starts out as a childless leaf + */ +TEST(TreeTest, AddSampleLinksToItsParent) { + RRTTree tree(point(0, 0)); + const RRTPoint sample = point(10, 10); + + tree.addSample(0, sample, option(14.0)); + + ASSERT_EQ(tree.tree.size, 2); + EXPECT_TRUE(tree.tree.points[1] == sample); + EXPECT_EQ(tree.tree.parent[1], 0); + EXPECT_EQ(tree.tree.first_child[1], INVALID_NODE); + EXPECT_EQ(tree.tree.next_sibling[1], INVALID_NODE); + EXPECT_EQ(tree.tree.rrt_options[1].length, 14.0); + + // the root now points at it + EXPECT_EQ(tree.tree.first_child[0], 1); + EXPECT_EQ(childrenOf(tree, 0), std::vector({1})); +} + +/* + * Slots are handed out in order -- RRT relies on this to know the id a sample + * will land at before it is added + */ +TEST(TreeTest, NodeIdsAreHandedOutInOrder) { + RRTTree tree(point(0, 0)); + + for (NodeId expected = 1; expected < 10; expected++) { + EXPECT_EQ(tree.tree.size, expected); + tree.addSample(0, point(expected, 0), option(1.0)); + EXPECT_TRUE(tree.tree.points[expected] == point(expected, 0)); + } +} + +/* + * Siblings are appended to the end of the list, so children stay in insertion order + */ +TEST(TreeTest, ChildrenKeepInsertionOrder) { + RRTTree tree(point(0, 0)); + + tree.addSample(0, point(1, 0), option(1.0)); + tree.addSample(0, point(2, 0), option(2.0)); + tree.addSample(0, point(3, 0), option(3.0)); + + EXPECT_EQ(childrenOf(tree, 0), std::vector({1, 2, 3})); + + // every child knows the root as its parent, and none of them have children + for (const NodeId child : childrenOf(tree, 0)) { + EXPECT_EQ(tree.tree.parent[child], 0); + EXPECT_EQ(tree.tree.first_child[child], INVALID_NODE); + } + + // a child of a child does not end up in the root's list + tree.addSample(2, point(2, 1), option(1.0)); + EXPECT_EQ(childrenOf(tree, 0), std::vector({1, 2, 3})); + EXPECT_EQ(childrenOf(tree, 2), std::vector({4})); +} + +/* + * A node's length is the distance flown from the root to reach it + */ +TEST(TreeTest, LengthAccumulatesDownABranch) { + RRTTree tree(point(0, 0)); + + tree.addSample(0, point(1, 0), option(10.0)); // node 1 + tree.addSample(1, point(2, 0), option(2.5)); // node 2 + tree.addSample(2, point(3, 0), option(7.5)); // node 3 + + // a second branch off of the root, to make sure lengths are not shared + tree.addSample(0, point(0, 1), option(100.0)); // node 4 + + EXPECT_DOUBLE_EQ(tree.tree.length[1], 10.0); + EXPECT_DOUBLE_EQ(tree.tree.length[2], 12.5); + EXPECT_DOUBLE_EQ(tree.tree.length[3], 20.0); + EXPECT_DOUBLE_EQ(tree.tree.length[4], 100.0); +} + +/* + * RRTTree::findPathToNode -- the options are returned in flight order + */ +TEST(TreeTest, FindPathToNodeReturnsOptionsInFlightOrder) { + RRTTree tree(point(0, 0)); + + tree.addSample(0, point(1, 0), option(10.0)); // node 1 + tree.addSample(1, point(2, 0), option(20.0)); // node 2 + tree.addSample(2, point(3, 0), option(30.0)); // node 3 + + EXPECT_EQ(lengthsOf(tree.findPathToNode(3)), std::vector({10.0, 20.0, 30.0})); + EXPECT_EQ(lengthsOf(tree.findPathToNode(2)), std::vector({10.0, 20.0})); + EXPECT_EQ(lengthsOf(tree.findPathToNode(1)), std::vector({10.0})); +} + +/* + * RRTTree::findPathToNode -- every segment carries the vector its option lands + * on, which is the point stored on the node the option belongs to + */ +TEST(TreeTest, FindPathToNodeCarriesTheEndPoints) { + RRTTree tree(point(0, 0)); + + tree.addSample(0, point(1, 0), option(10.0)); // node 1 + tree.addSample(1, point(2, 0), option(20.0)); // node 2 + tree.addSample(2, point(3, 0), option(30.0)); // node 3 + + const std::vector segments = tree.findPathToNode(3); + + ASSERT_EQ(segments.size(), 3); + for (NodeId node = 1; node <= 3; node++) { + EXPECT_TRUE(segments[node - 1].end == tree.tree.points[node]); + } +} + +/* + * RRTTree::findPathToNode -- only the target's own ancestors are on the path + */ +TEST(TreeTest, FindPathToNodeOnlyWalksAncestors) { + RRTTree tree(point(0, 0)); + + tree.addSample(0, point(1, 0), option(10.0)); // node 1, on the path + tree.addSample(0, point(0, 1), option(50.0)); // node 2, a sibling branch + tree.addSample(2, point(0, 2), option(60.0)); // node 3, hangs off the sibling + tree.addSample(1, point(2, 0), option(20.0)); // node 4, the target + + EXPECT_EQ(lengthsOf(tree.findPathToNode(4)), std::vector({10.0, 20.0})); + EXPECT_EQ(lengthsOf(tree.findPathToNode(3)), std::vector({50.0, 60.0})); +} + +/* + * RRTTree::findPathToNode -- the root is where the path starts, so it flies nothing + */ +TEST(TreeTest, FindPathToNodeHandlesRootAndInvalidNode) { + RRTTree tree(point(0, 0)); + tree.addSample(0, point(1, 0), option(10.0)); - Polygon obs1 = { - {XYZCoord(10, 10, 0), XYZCoord(20, 10, 0), XYZCoord(20, 20, 0), XYZCoord(10, 20, 0)}}; + EXPECT_TRUE(tree.findPathToNode(0).empty()); + EXPECT_TRUE(tree.findPathToNode(INVALID_NODE).empty()); +} - std::vector obstacles = {obs1}; - Environment::init(valid_region, {}, {}, obstacles); - RRTPoint point1 = RRTPoint(XYZCoord(25, 25, 0), 0); - RRTPoint point2 = RRTPoint(XYZCoord(50, 75, 0), 0); - RRTOption option = Dubins::bestOption(point1, point2); +/* + * RRTTree::setCurrentHead -- the tree is thrown away and restarted at the new head + */ +TEST(TreeTest, SetCurrentHeadRestartsTheTree) { + RRTTree tree(point(0, 0)); - RRTTree simple_tree = RRTTree(point1); + tree.addSample(0, point(1, 0), option(10.0)); + tree.addSample(1, point(2, 0), option(20.0)); + tree.addSample(0, point(0, 1), option(30.0)); + ASSERT_EQ(tree.tree.size, 4); - std::shared_ptr root = simple_tree.getRoot(); + const RRTPoint new_head(XYZCoord(50, 50, 0), M_PI); + tree.setCurrentHead(new_head); - // simpleTree.addNode(root, point1); - std::shared_ptr added_point = simple_tree.addSample(root, point2, option); + EXPECT_EQ(tree.tree.size, 1); + EXPECT_TRUE(tree.getStart() == new_head); + EXPECT_EQ(tree.tree.parent[0], INVALID_NODE); + EXPECT_EQ(tree.tree.first_child[0], INVALID_NODE); + EXPECT_EQ(tree.tree.next_sibling[0], INVALID_NODE); + EXPECT_EQ(tree.tree.length[0], 0.0); + EXPECT_TRUE(tree.findPathToNode(0).empty()); - EXPECT_TRUE(added_point != nullptr); - EXPECT_TRUE(root->getReachable().size() == 1); - EXPECT_TRUE(root->getReachable()[0]->getPoint() == point2); + // the slots the old tree used are handed back out + tree.addSample(0, point(51, 50), option(1.0)); + EXPECT_EQ(tree.tree.size, 2); + EXPECT_EQ(tree.tree.parent[1], 0); + EXPECT_DOUBLE_EQ(tree.tree.length[1], 1.0); + EXPECT_EQ(childrenOf(tree, 0), std::vector({1})); } -TEST(SimpleTreeTest, rewireEdgeTest) { - setDubins(5, 0.1); - Polygon valid_region; - valid_region.emplace_back(XYZCoord(0, 0, 0)); - valid_region.emplace_back(XYZCoord(100, 0, 0)); - valid_region.emplace_back(XYZCoord(100, 100, 0)); - valid_region.emplace_back(XYZCoord(0, 100, 0)); - Polygon obs1 = { - {XYZCoord(10, 10, 0), XYZCoord(20, 10, 0), XYZCoord(20, 20, 0), XYZCoord(10, 20, 0)}}; +/* + * The tree fills up to its capacity, one node per sample + */ +TEST(TreeTest, FillsToCapacity) { + RRTTree tree(point(0, 0)); - std::vector obstacles = {obs1}; - Environment::init(valid_region, {}, {}, obstacles); - RRTPoint point1 = RRTPoint(XYZCoord(25, 25, 0), 0); - RRTPoint point2 = RRTPoint(XYZCoord(50, 75, 0), HALF_PI); - RRTPoint point3 = RRTPoint(XYZCoord(50, 80, 1.5), HALF_PI); - RRTPoint point4 = RRTPoint(XYZCoord(50, 60, 0.9), HALF_PI); + // the root already took a slot + for (NodeId i = 1; i < ITERATIONS_PER_WAYPOINT; i++) { + tree.addSample(i - 1, point(i, 0), option(1.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]; + EXPECT_EQ(tree.tree.size, ITERATIONS_PER_WAYPOINT); + EXPECT_DOUBLE_EQ(tree.tree.length[ITERATIONS_PER_WAYPOINT - 1], ITERATIONS_PER_WAYPOINT - 1); + EXPECT_EQ(tree.findPathToNode(ITERATIONS_PER_WAYPOINT - 1).size(), + ITERATIONS_PER_WAYPOINT - 1); +} - RRTTree simple_tree = RRTTree(point1); +/* + * The path findPathToNode hands back, flown from the head, actually arrives at + * the node it was asked for + */ +TEST(TreeTest, PathToNodeFlownFromHeadReachesTheNode) { + setDubins(5, 1); - std::shared_ptr root = simple_tree.getRoot(); + const RRTPoint root_point(XYZCoord(0, 0, 0), 0); + RRTTree tree(root_point); - // these two should add - std::shared_ptr node2 = simple_tree.addSample(root, point2, option1); - std::shared_ptr node3 = simple_tree.addSample(node2, point3, option2); - std::shared_ptr node4 = simple_tree.addSample(root, point4, option3); - EXPECT_TRUE(node2 != nullptr); - EXPECT_TRUE(node3 != nullptr); - EXPECT_TRUE(node4 != nullptr); + const std::vector samples = { + RRTPoint(XYZCoord(30, 10, 0), M_PI / 4), + RRTPoint(XYZCoord(60, 40, 0), M_PI / 2), + RRTPoint(XYZCoord(20, 70, 0), M_PI), + }; - simple_tree.rewireEdge(node3, node2, node4, {}, 2); + NodeId parent = 0; + for (const RRTPoint& sample : samples) { + tree.addSample(parent, sample, Dubins::bestOption(tree.tree.points[parent], sample)); + parent = tree.tree.size - 1; + } - EXPECT_TRUE(node3->getPathLength() != 0); + const std::vector path = + Dubins::generatePath(tree.getStart(), tree.findPathToNode(parent)); - // EXPECT_TRUE(simple_tree.getEdge(node2, node3).getCost() == - // std::numeric_limits::infinity()); + ASSERT_FALSE(path.empty()); + EXPECT_NEAR(path.back().x, samples.back().coord.x, 0.01); + EXPECT_NEAR(path.back().y, samples.back().coord.y, 0.01); - EXPECT_TRUE(node4->getReachable().size() == 1); - EXPECT_TRUE(node2->getReachable().size() == 0); - EXPECT_TRUE(root->getReachable().size() == 2); -} \ No newline at end of file + // the length stored on the node is the length of that path + double flown = tree.getStart().coord.distanceTo(path[0]); + for (std::size_t i = 1; i < path.size(); i++) { + flown += path[i - 1].distanceTo(path[i]); + } + // the generated points cut corners off of every arc, so the polyline is a + // little shorter than the arc length the tree tracks + EXPECT_LT(flown, tree.tree.length[parent] + 0.01); + EXPECT_GT(flown, tree.tree.length[parent] * 0.98); +}