Merge pull request #337 from tritonuas/chore/remove-naive-pointers

Chore/remove naive pointers

Author: AskewParity

include/pathing/static.hpp

@@ -93,8 +93,16 @@ class RRT {
      * 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(RRTNode *goal_node, int current_goal_index);
+    bool epochEvaluation(std::shared_ptr<RRTNode> goal_node,
+                         std::shared_ptr<RRTNode> goal_parent,
+                         int current_goal_index);
 
     /**
      * Generates a random point in the airspace (uniformly)
@@ -111,11 +119,10 @@ class RRT {
      * connect to
      * @param total_options         ==> number of options to try to connect to the goal
      * @return                      ==> list of options to connect to the goal
-     *                                  <RRTPoint GOAL, {RRTNode* ANCHOR,
-     * RRTOption} >
+     *                                  <RRTPoint GOAL, {RRTNode* ANCHOR, RRTOption} >
      */
-    std::vector<std::pair<RRTPoint, std::pair<RRTNode *, RRTOption>>> getOptionsToGoal(
-        int current_goal_index, int total_options) const;
+    std::vector<std::pair<RRTPoint, std::pair<std::shared_ptr<RRTNode>, 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
@@ -124,10 +131,13 @@ class RRT {
      * @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
      */
-    RRTNode *sampleToGoal(int current_goal_index, int total_options) const;
+    std::shared_ptr<RRTNode> sampleToGoal(int current_goal_index,
+                                          int total_options,
+                                          std::shared_ptr<RRTNode>& parent) const;
 
     /**
      * Connects to the goal after RRT is finished
@@ -148,9 +158,12 @@ class RRT {
      *      - 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(RRTNode *goal_node, int current_goal_index);
+    void addNodeToTree(std::shared_ptr<RRTNode> goal_node,
+                       std::shared_ptr<RRTNode> parent,
+                       int current_goal_index);
 
     /**
      * Goes through generated options to try to connect the sample to the tree
@@ -160,8 +173,9 @@ class RRT {
      * @return          ==> whether or not the sample was successfully added to
      * the tree (nullptr if not added)
      */
-    RRTNode *parseOptions(const std::vector<std::pair<RRTNode *, RRTOption>> &options,
-                          const RRTPoint &sample);
+    std::shared_ptr<RRTNode> parseOptions(
+                        const std::vector<std::pair<std::shared_ptr<RRTNode>, RRTOption>> &options,
+                        const RRTPoint &sample);
 
     /**
      * Rewires the tree by finding paths that are more efficintly routed through
@@ -170,7 +184,7 @@ class RRT {
      *
      * @param sample    ==> sampled point
      */
-    void optimizeTree(RRTNode *sample);
+    void optimizeTree(std::shared_ptr<RRTNode> sample);
 };
 
 /**