RRT Data Structure Preparation

[Tyler-Lentz]

🚀 Feature

In order to implement the RRT* algorithm, first we need to get some data structures in place that will let us build an RRT tree.

This data structure should be declared in include/pathing/tree.hpp and defined in src/pathing/tree.cpp.

Inspired by the old implementation here and here.

There are four classes that will be necessary

• RRTPoint

  • Needs to have an XYZPoint inside of it, and a Psi value for angle

• RRTNode

  • needs to have an RRTPoint
  • needs to have a list of node pointers that are reachable from this node
  • needs to have a cost associated with reaching the node
  • needs to have a pointer to parent node

• RRTEdge

  • needs to have the starting node and ending node (pointers)
  • needs to have the path between the Nodes, a list of X,Y,Z points
  • needs to have a cost associated with traversing the edge

• RRTTree

  • essentially orchestrates all of the nodes and edges and allows operations on the tree, keeping everything consistent
  • nodes: map from XYZPoint to pointer to node
  • edges: map from pair of node pointers to edge
  • operations necessary:
    • add node, connected to previous node already in existence, implictly add new edge
    • rewire edge: given node1*, node2*, node3* where node1 is connected to node2, remove that connection and connect node1 to node3
    • access node pointer from XYZ point (getter), nullptr if does not exist
    • access edge from two XYZ point (getter), nullptr if does not exist

Documentation

See Tyler Once this is done to discuss what is necessary to document in wiki

Testing

Create unit tests building up simple and complex trees making sure the operations are working as expected. See Tyler for mroe guidance if needed.