Skip to content

Rewrite of pathing code #308

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Draft
CSTBook wants to merge 3 commits into
base: main
Choose a base branch
from
Draft

Rewrite of pathing code #308

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
9102cb5
Initial nuking
CSTBook Nov 1, 2025
a06a5a9
Replaced RRTNode* with a smart pointer and removed all references to …
CSTBook Nov 2, 2025
4eed901
small changes
CSTBook Nov 5, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
43 changes: 1 addition & 42 deletions include/pathing/environment.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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<XYZCoord>& goals,
const std::vector<Polygon>& 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<XYZCoord>& goals);

/**
* Check if an entire flight path is in bounds
Expand All @@ -49,20 +35,6 @@ class Environment {
*/
bool isPathInBounds(const std::vector<XYZCoord>& 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<XYZCoord>& path, const RRTOption& option) const;

/**
* Get the goal point
* can be unsafe if goals_found is not in bounds
Expand Down Expand Up @@ -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
*
Expand Down Expand Up @@ -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<XYZCoord> goals; // goal point
const std::vector<Polygon> 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
Expand Down
154 changes: 24 additions & 130 deletions include/pathing/tree.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,7 @@
#include <unordered_map>
#include <utility>
#include <vector>
#include <memory>

#include "pathing/dubins.hpp"
#include "pathing/environment.hpp"
Expand All @@ -14,15 +15,14 @@
#include "utilities/rng.hpp"

class RRTNode;
typedef std::vector<RRTNode*> RRTNodeList;
typedef XYZCoord Vector;
typedef std::shared_ptr<RRTNode> RRTNodePtr;

class RRTNode {
public:
RRTNode(const RRTPoint& point, double cost, double path_length,
const std::vector<XYZCoord> path);
RRTNode(const RRTPoint& point, double cost, double path_length,
const std::vector<XYZCoord> path, RRTNodeList reachable);
const std::vector<XYZCoord> path, std::vector<RRTNodePtr> reachable);

/**
* Destructor for RRTNode object
Expand All @@ -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<RRTNodePtr>& 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<RRTNodePtr>& getReachable();

/*
* Get the cost associated with this node
Expand All @@ -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
*
Expand Down Expand Up @@ -117,10 +103,9 @@ class RRTNode {

private:
RRTPoint point;
RRTNodeList reachable{};
std::vector<RRTNodePtr> reachable{};
double cost;
double path_length;
RRTNode* parent{};
std::vector<XYZCoord> path{};
};

Expand All @@ -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;

/**
Expand All @@ -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
Expand Down Expand Up @@ -184,7 +169,7 @@ class RRTTree {
*/
RRTPoint getRandomPoint(double search_radius) const;

bool validatePath(const std::vector<Vector>& path, const RRTOption& options) const;
bool validatePath(const std::vector<XYZCoord>& path, const RRTOption& options) const;

/**
* Returns a sorted list of the paths to get from a given node to the sampled
Expand All @@ -196,23 +181,16 @@ class RRTTree {
* function
* @return ==> mininum sorted list of pairs of <node, path>
*/
std::vector<std::pair<RRTNode*, RRTOption>> pathingOptions(
std::vector<std::pair<RRTNodePtr, 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
*
* @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 |_____
Expand All @@ -224,76 +202,23 @@ class RRTTree {
// */
// std::vector<XYZCoord> 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<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<RRTNode*> 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<RRTNode*>& 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<RRTNode*> 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<std::pair<double, RRTNode*>>& nodes_by_distance,
const RRTPoint& sample, RRTNode* 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<std::pair<RRTNode*, RRTOption>>& options,
const std::vector<RRTNode*>& nodes, const RRTPoint& sample) const;
void fillOptionsNodes(std::vector<std::pair<RRTNodePtr, RRTOption>>& options,
const std::vector<RRTNodePtr>& 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<XYZCoord> getPathSegment(RRTNode* node) const;
std::vector<XYZCoord> getPathSegment(RRTNodePtr node) const;

/**
* Returns the start RRTPoint
Expand All @@ -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;
Expand All @@ -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
Expand All @@ -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<std::pair<RRTNode*, RRTOption>>& options, RRTNode* node,
void fillOptions(std::vector<std::pair<RRTNodePtr, RRTOption>>& options, RRTNodePtr node,
const RRTPoint& sample) const;

/**
Expand All @@ -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<RRTNode*, 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(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<RRTNodePtr, double> getNearestNode(const XYZCoord& point) const;
};

#endif // INCLUDE_PATHING_TREE_HPP_
Loading
Loading