diff --git a/include/pathing/environment.hpp b/include/pathing/environment.hpp index 892b6194..0b826dab 100644 --- a/include/pathing/environment.hpp +++ b/include/pathing/environment.hpp @@ -17,25 +17,11 @@ * * [FUTURE] * - add dynamic shrinking and enlarging of the boundary - * - add dynamic obstacles */ class Environment { public: Environment(const Polygon& valid_region, const Polygon& airdrop_zone, - const Polygon& mapping_region, const std::vector& goals, - const std::vector& obstacles); - - /** - * Check if a point is in the valid region - * - * TODO - analysis if checking all regions for a given point at one time - * is optimal. The alternative would be to check each region individually - * for all points - * - * @param point the point to check - * @return true if the point is in the valid region, false otherwise - */ - bool isPointInBounds(const XYZCoord& point) const; + const Polygon& mapping_region, const std::vector& goals); /** * Check if an entire flight path is in bounds @@ -49,20 +35,6 @@ class Environment { */ bool isPathInBounds(const std::vector& path) const; - /** - * - * Check if an entire flight path is in bounds - * - * Attemps to skip a straight section by checking line segments instead of - * points, this doesn't actually end up making a large differernce with small - * path length? - * - * @param path the path to check - * @param option the RRT option associated with the path - * @return true if the path is in bounds, false otherwise - */ - bool isPathInBoundsAdv(const std::vector& path, const RRTOption& option) const; - /** * Get the goal point * can be unsafe if goals_found is not in bounds @@ -118,18 +90,6 @@ class Environment { */ static bool isPointInPolygon(const Polygon& polygon, const XYZCoord& point); - /** - * Checks wheter a line segment is in bounds or not, it must NOT intersect - * either the valid region or the obstacles - * - * ASSUMES THAT THE END POINTS ARE IN THE POLYGON - * - * @param start_point ==> start point of the line segment - * @param end_point ==> end point of the line segment - * @return ==> whether or not the line segment is in bounds - */ - bool isLineInBounds(const XYZCoord& start_point, const XYZCoord& end_point) const; - /** * Determines whether a line segment intersects the polygon * @@ -272,7 +232,6 @@ class Environment { const Polygon airdrop_zone; // boundary of the airdrop zone (subset of valid_region) const Polygon mapping_region; // boundary of the mapping region (subset of valid_region) const std::vector goals; // goal point - const std::vector obstacles; // obstacles in the map int goals_found; // whether or not the goal has been found, once it becomes ture, it will never // be false again diff --git a/include/pathing/tree.hpp b/include/pathing/tree.hpp index f834877b..97c2c15e 100644 --- a/include/pathing/tree.hpp +++ b/include/pathing/tree.hpp @@ -6,6 +6,7 @@ #include #include #include +#include #include "pathing/dubins.hpp" #include "pathing/environment.hpp" @@ -14,15 +15,14 @@ #include "utilities/rng.hpp" class RRTNode; -typedef std::vector RRTNodeList; -typedef XYZCoord Vector; +typedef std::shared_ptr RRTNodePtr; 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); + const std::vector path, std::vector reachable); /** * Destructor for RRTNode object @@ -44,24 +44,24 @@ class RRTNode { /* * Set the reachable (neighbors) list for this RRTNode object */ - void setReachable(const RRTNodeList& reachable); + void setReachable(const std::vector& reachable); /* * Add a new node to the end of this node's reachable list. * Set the new node's parent to be this node. */ - void addReachable(RRTNode* new_node); + void addReachable(RRTNodePtr new_node); /* * Remove a specific node from this node's reachable list. * Set the removed node's parent pointer to be null. */ - void removeReachable(RRTNode* old_node); + void removeReachable(RRTNodePtr old_node); /* * Return a reference to this node's reachable list */ - const RRTNodeList& getReachable(); + const std::vector& getReachable(); /* * Get the cost associated with this node @@ -73,20 +73,6 @@ class RRTNode { */ void setCost(double new_cost); - /** - * Get the parent of this node - * - * @return RRTNode* pointer to parent node - */ - RRTNode* getParent() const; - - /** - * Set the parent of this node - * - * @param new_parent pointer to new parent node - */ - void setParent(RRTNode* new_parent); - /** * Get the path associated with this node * @@ -117,10 +103,9 @@ class RRTNode { private: RRTPoint point; - RRTNodeList reachable{}; + std::vector reachable{}; double cost; double path_length; - RRTNode* parent{}; std::vector path{}; }; @@ -132,7 +117,7 @@ class RRTTree { /** * Generates node without adding it to the tree */ - RRTNode* generateNode(RRTNode* anchor_node, const RRTPoint& new_point, + RRTNodePtr generateNode(RRTNodePtr anchor_node, const RRTPoint& new_point, const RRTOption& option) const; /** @@ -141,20 +126,20 @@ class RRTTree { * @param anchor_node ==> the node to connect to * @param new_node ==> the node to add */ - bool addNode(RRTNode* anchor_node, RRTNode* new_node); + bool addNode(RRTNodePtr anchor_node, RRTNodePtr new_node); /* * Add a node to the RRTTree. * If adding the first node to the tree, connectTo can be anything. */ - RRTNode* addSample(RRTNode* anchor_node, const RRTPoint& new_point, const RRTOption& option); + RRTNodePtr addSample(RRTNodePtr anchor_node, const RRTPoint& new_point, const RRTOption& option); /** * Returns a pointer to the root node * - * @return RRTNode* pointer to root node + * @return RRTNodePtr pointer to root node */ - RRTNode* getRoot() const; + RRTNodePtr getRoot() const; /** * Get goal point @@ -184,7 +169,7 @@ class RRTTree { */ RRTPoint getRandomPoint(double search_radius) const; - bool validatePath(const std::vector& path, const RRTOption& options) const; + bool validatePath(const std::vector& path, const RRTOption& options) const; /** * Returns a sorted list of the paths to get from a given node to the sampled @@ -196,23 +181,16 @@ class RRTTree { * function * @return ==> mininum sorted list of pairs of */ - std::vector> pathingOptions( + std::vector> 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 - * - * @param sample ==> the point to used as the base - * @param rewire_radius ==> the radius to search for nodes to rewire - */ - void RRTStar(RRTNode* sample, double rewire_radius); - /** * Changes the currentHead to the given goal * * @param goal ==> the goal to change the currentHead to */ - void setCurrentHead(RRTNode* goal); + void setCurrentHead(RRTNodePtr goal); // /** // * _____| UNUSED |_____ @@ -224,59 +202,6 @@ class RRTTree { // */ // std::vector getPathToGoal() const; - /** - * 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(RRTNode* current_point, RRTNode* previous_parent, RRTNode* 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, RRTNode* 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 - */ - 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, RRTNode* current_node) const; - /** * Fills in a list of options from an existing list of nodes * @@ -284,8 +209,8 @@ class RRTTree { * @param nodes ==> the list of nodes to parse * @param sample ==> the end point that the options will be connected to */ - void fillOptionsNodes(std::vector>& options, - const std::vector& nodes, const RRTPoint& sample) const; + void fillOptionsNodes(std::vector>& options, + const std::vector& nodes, const RRTPoint& sample) const; /** * Returns the segment of path from the given node to the current head @@ -293,7 +218,7 @@ class RRTTree { * @param node ==> the node to start the path from * @return ==> the path from the node to the current head */ - std::vector getPathSegment(RRTNode* node) const; + std::vector getPathSegment(RRTNodePtr node) const; /** * Returns the start RRTPoint @@ -303,8 +228,8 @@ class RRTTree { RRTPoint& getStart() const; private: - RRTNode* root; - RRTNode* current_head; + RRTNodePtr root; + RRTNodePtr current_head; Environment airspace; Dubins dubins; int tree_size; @@ -314,7 +239,7 @@ class RRTTree { * * @param node ==> the root of the tree to delete */ - void deleteTree(RRTNode* node); + void deleteTree(RRTNodePtr node); /** * traverses the tree, and puts in all RRTOptions from dubins into a list @@ -324,7 +249,7 @@ class RRTTree { * @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>& options, RRTNode* node, + void fillOptions(std::vector>& options, RRTNodePtr node, const RRTPoint& sample) const; /** @@ -333,38 +258,7 @@ class RRTTree { * @param point ==> the point to find the nearest node to * @return ==> the nearest node to the point */ - // std::pair 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(RRTNode* current_node, RRTNode* sample, double rewire_radius); - - /** - * After rewire edge, it goes down the tree and reassigns the cost of the - * nodes - * - * @param changed_node the node that has been changed - */ - void reassignCosts(RRTNode* 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(RRTNode* parent, RRTNode* current_node, double cost_difference); + // std::pair getNearestNode(const XYZCoord& point) const; }; #endif // INCLUDE_PATHING_TREE_HPP_ diff --git a/include/utilities/obc_config.hpp b/include/utilities/obc_config.hpp index 54e2b877..f9425a5f 100644 --- a/include/utilities/obc_config.hpp +++ b/include/utilities/obc_config.hpp @@ -57,15 +57,15 @@ struct DubinsConfig { 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; -}; +// 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* //DELETE +// bool optimize; // run RRT* if true //DELETE +// PointFetchMethod::Enum point_fetch_method; // DELETE +// bool allowed_to_skip_waypoints; // if true, will skip waypoints if it can not connect after 1 +// // RRT iteration +// bool generate_deviations; //DELETE +// }; namespace AirdropCoverageMethod { enum class Enum { HOVER, FORWARD }; diff --git a/src/pathing/environment.cpp b/src/pathing/environment.cpp index 60f19393..0493fd8f 100644 --- a/src/pathing/environment.cpp +++ b/src/pathing/environment.cpp @@ -10,30 +10,23 @@ #include "utilities/datatypes.hpp" #include "utilities/rng.hpp" + +/* +To nuke or not to nuke, that is the question +Whether 'tis nobler in the mind to suffer +The pointers and recursion of outrageous fortune, +Or to take arms against a sea of segfaults +And by opposing end them. +*/ + Environment::Environment(const Polygon& valid_region, const Polygon& airdrop_zone, - const Polygon& mapping_region, const std::vector& goals, - const std::vector& obstacles) + const Polygon& mapping_region, const std::vector& goals) : valid_region(valid_region), airdrop_zone(airdrop_zone), mapping_region(mapping_region), goals(goals), goals_found(0), - bounds(findBounds(valid_region)), - obstacles(obstacles) {} - -bool Environment::isPointInBounds(const XYZCoord& point) const { - if (!isPointInPolygon(valid_region, point)) { - return false; - } - - for (const Polygon& obstacle : obstacles) { - if (isPointInPolygon(obstacle, point)) { - return false; - } - } - - return true; -} + bounds(findBounds(valid_region)) {} bool Environment::isPathInBounds(const std::vector& path) const { /* @@ -48,7 +41,7 @@ bool Environment::isPathInBounds(const std::vector& path) const { const int interval = ENV_PATH_VALIDATION_STEP_SIZE; // special case not checked in the loop - if (!isPointInBounds(path[center])) { + if (!isPointInPolygon(valid_region, [center])) { return false; } @@ -57,7 +50,7 @@ bool Environment::isPathInBounds(const std::vector& path) const { // to the right while (count < path.size()) { - if (!isPointInBounds(path[count])) { + if (!isPointInPolygon(valid_region, path[count])) { return false; } count += interval; @@ -66,7 +59,7 @@ bool Environment::isPathInBounds(const std::vector& path) const { // to the left count = center - i; while (count >= 0) { - if (!isPointInBounds(path[count])) { + if (!isPointInPolygon(valid_region, path[count])) { return false; } count -= interval; @@ -76,44 +69,6 @@ bool Environment::isPathInBounds(const std::vector& path) const { return true; } -// bool Environment::isPathInBoundsAdv(const std::vector& path, -// const RRTOption& option) const { -// if (!option.has_straight) { -// return isPathInBounds(path); -// } - -// // finds the last point on the first curve, and the first point on the second curve -// // does this using the option, using arclength and the point separation -// const int first_curve_end = -// std::abs(option.dubins_path.beta_0) * TURNING_RADIUS / POINT_SEPARATION + 1; -// const int second_curve_start = -// path.size() - std::abs(option.dubins_path.beta_2) * TURNING_RADIUS / POINT_SEPARATION; - -// // sanity check -// if (first_curve_end >= second_curve_start) { -// return isPathInBounds(path); -// } - -// if (!isLineInBounds(path[first_curve_end], path[second_curve_start])) { -// return false; -// } - -// // checks the points manually in the curve -// for (int i = 0; i <= first_curve_end; i++) { -// if (!isPointInBounds(path[i])) { -// return false; -// } -// } - -// for (int i = second_curve_start; i < path.size(); i++) { -// if (!isPointInBounds(path[i])) { -// return false; -// } -// } - -// return true; -// } - const XYZCoord& Environment::getGoal() const { return goals[goals_found]; } const XYZCoord& Environment::getGoal(int index) const { return goals[index]; } @@ -121,13 +76,14 @@ const XYZCoord& Environment::getGoal(int index) const { return goals[index]; } XYZCoord Environment::getRandomPoint(bool use_mapping_region = false) const { // TODO - use some heuristic to more efficiently generate direction // vector (and make it toggleable) + // potentially utilize ellipse sampling or goal biasing std::pair, std::pair> polygon_bounds = bounds; if (use_mapping_region) { polygon_bounds = findBounds(mapping_region); } for (int i = 0; i < TRIES_FOR_RANDOM_POINT; i++) { - // generates a random point in the rectangle contianing the valid region + // generates a random point in the rectangle containing the valid region XYZCoord generated_point = { random(polygon_bounds.first.first, polygon_bounds.first.second), random(polygon_bounds.second.first, polygon_bounds.second.second), 0}; @@ -137,7 +93,7 @@ XYZCoord Environment::getRandomPoint(bool use_mapping_region = false) const { return generated_point; } } else { - if (isPointInBounds(generated_point)) { + if (isPointInPolygon(valid_region, generated_point)) { return generated_point; } } @@ -164,20 +120,6 @@ bool Environment::isPointInPolygon(const Polygon& polygon, const XYZCoord& point return is_inside; } -bool Environment::isLineInBounds(const XYZCoord& start_point, const XYZCoord& end_point) const { - if (doesLineIntersectPolygon(start_point, end_point, valid_region)) { - return false; - } - - for (const Polygon& obstacle : obstacles) { - if (doesLineIntersectPolygon(start_point, end_point, obstacle)) { - return false; - } - } - - return true; -} - bool Environment::doesLineIntersectPolygon(const XYZCoord& start_point, const XYZCoord& end_point, const Polygon& polygon) const { for (int i = 0, j = polygon.size() - 1; i < polygon.size(); j = i++) { diff --git a/src/pathing/static.cpp b/src/pathing/static.cpp index eedf4b05..8e77377e 100644 --- a/src/pathing/static.cpp +++ b/src/pathing/static.cpp @@ -20,269 +20,280 @@ #include "utilities/obc_config.hpp" #include "utilities/rng.hpp" -RRT::RRT(RRTPoint start, std::vector goals, double search_radius, Polygon bounds, - const OBCConfig &config, std::vector obstacles, std::vector angles) - : iterations_per_waypoint(config.pathing.rrt.iterations_per_waypoint), - search_radius(search_radius), - rewire_radius(config.pathing.rrt.rewire_radius), - tree(start, Environment(bounds, {}, {}, goals, obstacles), - Dubins(config.pathing.dubins.turning_radius, config.pathing.dubins.point_separation)), - config(config.pathing.rrt) { - if (angles.size() != 0) { - this->angles = angles; - } -} - -RRT::RRT(RRTPoint start, std::vector goals, double search_radius, Environment airspace, - const OBCConfig &config, std::vector angles) - : iterations_per_waypoint(config.pathing.rrt.iterations_per_waypoint), - search_radius(search_radius), - rewire_radius(config.pathing.rrt.rewire_radius), - tree(start, airspace, - Dubins(config.pathing.dubins.turning_radius, config.pathing.dubins.point_separation)), - 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 = tree.getAirspace().getNumGoals(); - - 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; - - RRTNode *goal_node = 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, 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> &options = - tree.pathingOptions(sample, config.point_fetch_method); - - // returns true if the node is successfully added to the tree - RRTNode *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(RRTNode *goal_node, 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); - return false; - } - - RRTNode *new_node = sampleToGoal(current_goal_index, TOTAL_OPTIONS_FOR_GOAL_CONNECTION); - - 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; - return false; - } - - addNodeToTree(new_node, current_goal_index); - return true; -} - -RRTPoint RRT::generateSamplePoint() const { return tree.getRandomPoint(search_radius); } - -std::vector>> 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) { - const XYZCoord &goal = tree.getAirspace().getGoal(current_goal_index); - goal_points.push_back(RRTPoint(goal, 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>> 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> &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; -} - -RRTNode *RRT::sampleToGoal(int current_goal_index, int total_options) 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; - - RRTNode *new_node = tree.generateNode(anchor_node, goal, option); - - if (new_node != nullptr) { - return new_node; - } - } - - return nullptr; -} - -bool RRT::connectToGoal(int current_goal_index, int total_options) { - RRTNode *goal_node = sampleToGoal(current_goal_index, total_options); - - if (goal_node == nullptr) { - return false; - } - - addNodeToTree(goal_node, current_goal_index); - return true; -} - -void RRT::addNodeToTree(RRTNode *goal_node, int current_goal_index) { - // add the node to the tree - tree.addNode(goal_node->getParent(), 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 = tree.getAirspace().getGoal(current_goal_index - 1).z; - } - - double height_difference = tree.getAirspace().getGoal(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); -} - -RRTNode *RRT::parseOptions(const std::vector> &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 - RRTNode *sucessful_addition = tree.addSample(node, sample, option); - - if (sucessful_addition != nullptr) { - return sucessful_addition; - } - } - - return nullptr; -} - -void RRT::optimizeTree(RRTNode *sample) { tree.RRTStar(sample, rewire_radius); } +/* +I remained to dream the nightmare out to the end, and to show my loyalty to TUAS once more. +Path-planning. My Path-planning! Droll thing RRT is—that mysterious arrangement of merciless logic for a futile purpose. +The most you can hope from it is some knowledge of yourself—that comes too late—a crop of unextinguishable errors. +I have wrestled with C++. It is the most unexciting contest you can imagine. +*/ +// RRT::RRT(RRTPoint start, std::vector goals, double search_radius, Polygon bounds, +// const OBCConfig &config, std::vector obstacles, std::vector angles) +// : iterations_per_waypoint(config.pathing.rrt.iterations_per_waypoint), +// search_radius(search_radius), +// rewire_radius(config.pathing.rrt.rewire_radius), +// tree(start, Environment(bounds, {}, {}, goals, obstacles), +// Dubins(config.pathing.dubins.turning_radius, config.pathing.dubins.point_separation)), +// config(config.pathing.rrt) { +// if (angles.size() != 0) { +// this->angles = angles; +// } +// } + +// RRT::RRT(RRTPoint start, std::vector goals, double search_radius, Environment airspace, +// const OBCConfig &config, std::vector angles) +// : iterations_per_waypoint(config.pathing.rrt.iterations_per_waypoint), +// search_radius(search_radius), +// rewire_radius(config.pathing.rrt.rewire_radius), +// tree(start, airspace, +// Dubins(config.pathing.dubins.turning_radius, config.pathing.dubins.point_separation)), +// 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 = tree.getAirspace().getNumGoals(); + +// 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; + +// RRTNode *goal_node = 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, 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> &options = +// tree.pathingOptions(sample, config.point_fetch_method); + +// // returns true if the node is successfully added to the tree +// RRTNode *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; +// } + +// This code will disappear, but not immediately. Deo volente, it will provide a useful framework. + +// bool RRT::epochEvaluation(RRTNode *goal_node, 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); +// return false; +// } + +// RRTNode *new_node = sampleToGoal(current_goal_index, TOTAL_OPTIONS_FOR_GOAL_CONNECTION); + +// 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; +// return false; +// } + +// addNodeToTree(new_node, current_goal_index); +// return true; +// } + +// RRTPoint RRT::generateSamplePoint() const { return tree.getRandomPoint(search_radius); } + +// std::vector>> 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) { +// const XYZCoord &goal = tree.getAirspace().getGoal(current_goal_index); +// goal_points.push_back(RRTPoint(goal, 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>> 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> &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; +// } + +// RRTNode *RRT::sampleToGoal(int current_goal_index, int total_options) 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; + +// RRTNode *new_node = tree.generateNode(anchor_node, goal, option); + +// if (new_node != nullptr) { +// return new_node; +// } +// } + +// return nullptr; +// } + +// bool RRT::connectToGoal(int current_goal_index, int total_options) { +// RRTNode *goal_node = sampleToGoal(current_goal_index, total_options); + +// if (goal_node == nullptr) { +// return false; +// } + +// addNodeToTree(goal_node, current_goal_index); +// return true; +// } + +// void RRT::addNodeToTree(RRTNode *goal_node, int current_goal_index) { +// // add the node to the tree +// tree.addNode(goal_node->getParent(), 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 = tree.getAirspace().getGoal(current_goal_index - 1).z; +// } + +// double height_difference = tree.getAirspace().getGoal(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); +// } + +// RRTNode *RRT::parseOptions(const std::vector> &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 +// RRTNode *sucessful_addition = tree.addSample(node, sample, option); + +// if (sucessful_addition != nullptr) { +// return sucessful_addition; +// } +// } + +// return nullptr; +// } + +// void RRT::optimizeTree(RRTNode *sample) { tree.RRTStar(sample, rewire_radius); } + + +// sanctuary of safety ForwardCoveragePathing::ForwardCoveragePathing(const RRTPoint &start, double scan_radius, Polygon bounds, Polygon airdrop_zone, const OBCConfig &config, @@ -594,6 +605,7 @@ std::vector> generateRankedNewGoalsList(const std::vector< 2. we omit the first waypoint (FATAL) - this means we never tell the computer to hit it 3. We don't know where home location is - we rely on geofencing to not fly out of bounds */ +// DO NOT TOUCH THIS MissionPath generateInitialPath(std::shared_ptr state) { // first waypoint is start @@ -609,16 +621,20 @@ MissionPath generateInitialPath(std::shared_ptr state) { // Copy elements from the second element to the last element of source into // destination all other methods of copying over crash??? + + // dont start second, incorrect asumption. Change in the RRT object for (int i = 1; i < state->mission_params.getWaypoints().size(); i++) { goals.emplace_back(state->mission_params.getWaypoints()[i]); } // update goals here + // DELETE if (state->config.pathing.rrt.generate_deviations) { Environment mapping_bounds(state->mission_params.getMappingBoundary(), {}, {}, goals, {}); goals = generateRankedNewGoalsList(goals, mapping_bounds)[0]; } + // assumes that first waypoint is home pos double init_angle = std::atan2(goals.front().y - state->mission_params.getWaypoints().front().y, goals.front().x - state->mission_params.getWaypoints().front().x); @@ -640,6 +656,7 @@ MissionPath generateInitialPath(std::shared_ptr state) { return MissionPath(MissionPath::Type::FORWARD, output_coords); } +// DO NOT TOUCH THIS MissionPath generateSearchPath(std::shared_ptr state) { std::vector gps_coords; if (state->config.pathing.coverage.method == AirdropCoverageMethod::Enum::FORWARD) { @@ -669,6 +686,7 @@ MissionPath generateSearchPath(std::shared_ptr state) { } } +// DO NOT TOUCH MissionPath generateAirdropApproach(std::shared_ptr state, const GPSCoord &goal) { // finds starting location std::shared_ptr mav = state->getMav(); diff --git a/src/pathing/tree.cpp b/src/pathing/tree.cpp index f330f70e..bbe1a31a 100644 --- a/src/pathing/tree.cpp +++ b/src/pathing/tree.cpp @@ -11,18 +11,18 @@ #include "utilities/rng.hpp" #include "utilities/obc_config.hpp" +// When we are born, we cry that we are come to this great stage of fools + RRTNode::RRTNode(const RRTPoint& point, double cost, double path_length, const std::vector path) : point{point}, cost{cost}, path_length(path_length), path(path) {} RRTNode::RRTNode(const RRTPoint& point, double cost, double path_length, - const std::vector path, RRTNodeList reachable) + const std::vector path, std::vector reachable) : point{point}, cost{cost}, path_length(path_length), path(path), reachable{reachable} {} RRTNode::~RRTNode() { - for (RRTNode* node : reachable) { - delete node; - } + reachable.clear(); } // bool RRTNode::operator==(const RRTNode& other_node) const { @@ -31,38 +31,28 @@ RRTNode::~RRTNode() { RRTPoint& RRTNode::getPoint() { return this->point; } -void RRTNode::setReachable(const RRTNodeList& reachable) { +void RRTNode::setReachable(const std::vector& reachable) { this->reachable = reachable; - for (RRTNode* node : reachable) { - node->parent = this; - } } - -void RRTNode::addReachable(RRTNode* new_node) { +void RRTNode::addReachable(RRTNodePtr new_node) { this->reachable.push_back(new_node); - new_node->parent = this; } -void RRTNode::removeReachable(RRTNode* old_node) { +void RRTNode::removeReachable(RRTNodePtr old_node) { for (int i = 0; i < reachable.size(); i++) { if (reachable.at(i) == old_node) { - // TODO - UNSAFE - reachable.at(i)->parent = nullptr; reachable.erase(reachable.begin() + i); + return; } } } -const RRTNodeList& RRTNode::getReachable() { return (this->reachable); } +const std::vector& RRTNode::getReachable() { return (this->reachable); } double RRTNode::getCost() const { return this->cost; } void RRTNode::setCost(double new_cost) { this->cost = new_cost; } -RRTNode* RRTNode::getParent() const { return this->parent; } - -void RRTNode::setParent(RRTNode* new_parent) { this->parent = new_parent; } - const std::vector& RRTNode::getPath() const { return this->path; } void RRTNode::setPath(const std::vector& path) { this->path = path; } @@ -70,36 +60,26 @@ 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, Environment airspace, Dubins dubins) : airspace(airspace), dubins(dubins), tree_size(1) { - RRTNode* new_node = new RRTNode(root_point, 0, 0, {}); + RRTNodePtr new_node = std::make_shared(root_point, 0, 0, {}); root = new_node; current_head = new_node; } -// TODO - seems a bit sketchy -RRTTree::~RRTTree() { delete root; } +RRTTree::~RRTTree() { root.reset(); } +// MAYBE: glorified function wrapper that uses one of its params, need to wipe. bool RRTTree::validatePath(const std::vector& path, const RRTOption& option) const { return airspace.isPathInBounds(path); - // return airspace.isPathInBoundsAdv(path, option); } -RRTNode* RRTTree::generateNode(RRTNode* anchor_node, const RRTPoint& new_point, +RRTNodePtr RRTTree::generateNode(RRTNodePtr 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); @@ -109,14 +89,13 @@ RRTNode* RRTTree::generateNode(RRTNode* anchor_node, const RRTPoint& new_point, } // needs to add the node to the tree - RRTNode* new_node = - new RRTNode(new_point, anchor_node->getCost() + option.length, option.length, path); - new_node->setParent(anchor_node); + RRTNodePtr new_node = + std::make_shared(new_point, anchor_node->getCost() + option.length, option.length, path); return new_node; } -bool RRTTree::addNode(RRTNode* anchor_node, RRTNode* new_node) { +bool RRTTree::addNode(RRTNodePtr anchor_node, RRTNodePtr new_node) { if (new_node == nullptr || anchor_node == nullptr) { return false; } @@ -127,9 +106,9 @@ bool RRTTree::addNode(RRTNode* anchor_node, RRTNode* new_node) { } // TODO - convert from old to new -RRTNode* RRTTree::addSample(RRTNode* anchor_node, const RRTPoint& new_point, +RRTNodePtr RRTTree::addSample(RRTNodePtr anchor_node, const RRTPoint& new_point, const RRTOption& option) { - RRTNode* new_node = generateNode(anchor_node, new_point, option); + RRTNodePtr new_node = generateNode(anchor_node, new_point, option); if (addNode(anchor_node, new_node)) { return new_node; @@ -138,85 +117,9 @@ RRTNode* RRTTree::addSample(RRTNode* anchor_node, const RRTPoint& new_point, return nullptr; } -void RRTTree::rewireEdge(RRTNode* current_node, RRTNode* previous_parent, RRTNode* 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, RRTNode* 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 (RRTNode* 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, RRTNode* 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 (RRTNode* node : current_node->getReachable()) { - getKClosestNodesRecursive(nodes_by_distance, sample, node); - } -} - -void RRTTree::fillOptionsNodes(std::vector>& options, - const std::vector& nodes, const RRTPoint& sample) const { - for (RRTNode* node : nodes) { +void RRTTree::fillOptionsNodes(std::vector>& options, + const std::vector& nodes, const RRTPoint& sample) const { + for (RRTNodePtr node : nodes) { const std::vector& local_options = dubins.allOptions(node->getPoint(), sample); for (const RRTOption& option : local_options) { @@ -230,7 +133,7 @@ void RRTTree::fillOptionsNodes(std::vector>& opti } } -RRTNode* RRTTree::getRoot() const { return this->root; } +RRTNodePtr RRTTree::getRoot() const { return this->root; } const XYZCoord& RRTTree::getGoal() const { return airspace.getGoal(); } @@ -242,10 +145,12 @@ RRTPoint RRTTree::getRandomPoint(double search_radius) const { // gets random point if the goal is not being used const XYZCoord& sample = airspace.getRandomPoint(); + // MAYBE: go through this code, if anything of value, take it, otherwise throw away + // // picks the nearest node to the sample, and then returns a point `search_radius` distance // away // // from tat point in the direction of the sample - // std::pair nearest_node = getNearestNode(sample); + // std::pair nearest_node = getNearestNode(sample); // int count = 0; @@ -286,27 +191,11 @@ RRTPoint RRTTree::getRandomPoint(double search_radius) const { /* TODO - investigate whether a max heap is better or worse */ -std::vector> RRTTree::pathingOptions( +std::vector> RRTTree::pathingOptions( const RRTPoint& end, PointFetchMethod::Enum point_fetch_method, int quantity_options) const { // fills the options list with valid values - std::vector> options; - - switch (point_fetch_method) { - case PointFetchMethod::Enum::RANDOM: { - const std::vector& nodes = getKRandomNodes(K_RANDOM_NODES); - fillOptionsNodes(options, nodes, end); - } break; - case PointFetchMethod::Enum::NEAREST: { - const std::vector& 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; - } + std::vector> options; + fillOptions(options, current_head, end); // sorts the list std::sort(options.begin(), options.end(), [](auto& a, auto& b) { @@ -331,7 +220,7 @@ std::vector> RRTTree::pathingOptions( return options; } -void RRTTree::fillOptions(std::vector>& options, RRTNode* node, +void RRTTree::fillOptions(std::vector>& options, RRTNodePtr node, const RRTPoint& end) const { /* TODO - try to limit the scope of the search to prevent too many calls to dubins @@ -353,17 +242,12 @@ void RRTTree::fillOptions(std::vector>& options, } // recursively calls the function for all reachable nodes - for (RRTNode* child : node->getReachable()) { + for (RRTNodePtr child : node->getReachable()) { fillOptions(options, child, end); } } -void RRTTree::RRTStar(RRTNode* 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(RRTNode* goal) { +void RRTTree::setCurrentHead(RRTNodePtr goal) { if (goal == nullptr) { LOG_F(ERROR, "FAILURE: Goal is not in the tree"); return; @@ -375,7 +259,7 @@ void RRTTree::setCurrentHead(RRTNode* goal) { } // std::vector RRTTree::getPathToGoal() const { -// RRTNode* current_node = current_head; +// RRTNodePtr current_node = current_head; // std::vector path = {}; // while (current_node != nullptr && current_node->getParent() != nullptr) { @@ -390,8 +274,8 @@ void RRTTree::setCurrentHead(RRTNode* goal) { // return path; // } -std::vector RRTTree::getPathSegment(RRTNode* node) const { - RRTNode* current_node = node; +std::vector RRTTree::getPathSegment(RRTNodePtr node) const { + RRTNodePtr current_node = node; std::vector path = {}; while (current_node != current_head) { @@ -411,101 +295,18 @@ std::vector RRTTree::getPathSegment(RRTNode* node) const { RRTPoint& RRTTree::getStart() const { return root->getPoint(); } -/*-----------------*/ -/* RRTTree Private */ -/*-----------------*/ - -// std::pair RRTTree::getNearestNode(const XYZCoord& point) const { -// RRTNode* nearest = nullptr; -// double min_distance = std::numeric_limits::infinity(); - -// for (auto& node : node_set) { -// double distance = point.distanceToSquared(node->getPoint().coord); -// if (distance < min_distance) { -// min_distance = distance; -// nearest = node; -// } -// } - -// return {nearest, min_distance}; -// } - -void RRTTree::RRTStarRecursive(RRTNode* current_node, RRTNode* sample, - double rewire_radius_squared) { - // base case - if (current_node == nullptr) { - return; - } - - // for all nodes past the current node, attempt to rewire them - for (RRTNode* child : current_node->getReachable()) { - // 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) - const std::vector& options = - dubins.allOptions(sample->getPoint(), child->getPoint(), true); - - // 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; - } - - // 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); - - if (!validatePath(path, option)) { - continue; - } - - // rewire the edge - rewireEdge(child, current_node, sample, path, option.length); - } - } - - // recurse - for (RRTNode* child : current_node->getReachable()) { - RRTStarRecursive(child, sample, rewire_radius_squared); - } -} - -void RRTTree::reassignCosts(RRTNode* changed_node) { - if (changed_node == nullptr) { - return; - } - - for (RRTNode* child : changed_node->getReachable()) { - reassignCostsRecursive(changed_node, child, changed_node->getCost()); - } -} - -void RRTTree::reassignCostsRecursive(RRTNode* parent, RRTNode* current_node, double path_cost) { - if (current_node == nullptr) { - return; - } - - // 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 (RRTNode* neighbor : current_node->getReachable()) { - reassignCostsRecursive(current_node, neighbor, current_node->getCost()); - } -} +// I'm keeping this code for the time being, because it's prolly useful for implementing RRT +// // std::pair RRTTree::getNearestNode(const XYZCoord& point) const { +// // RRTNodePtr nearest = nullptr; +// // double min_distance = std::numeric_limits::infinity(); + +// // for (auto& node : node_set) { +// // double distance = point.distanceToSquared(node->getPoint().coord); +// // if (distance < min_distance) { +// // min_distance = distance; +// // nearest = node; +// // } +// // } + +// // return {nearest, min_distance}; +// // } \ No newline at end of file