Replace Vector<T*> with Deque<T>

src/rrt-viewer/RRTWidget.cpp

@@ -261,15 +261,15 @@ void RRTWidget::drawTree(QPainter& painter, const Tree<Vector2f>& rrt,
     const float r = 1;
 
     //  draw all the nodes and connections
-    for (const Node<Vector2f>* node : rrt.allNodes()) {
+    for (const Node<Vector2f>& node : rrt.allNodes()) {
         painter.setPen(QPen(treeColor, 1));
-        QPointF loc = pointFromNode(node);
+        QPointF loc = pointFromNode(&node);
         painter.drawEllipse(loc, r, r);
 
-        if (node->parent()) {
+        if (node.parent()) {
             //  draw edge
             painter.setPen(QPen(treeColor, 1));
-            QPointF parentLoc = pointFromNode(node->parent());
+            QPointF parentLoc = pointFromNode(node.parent());
             painter.drawLine(loc, parentLoc);
         }
     }

src/rrt/BiRRT.hpp

@@ -101,7 +101,7 @@ class BiRRT {
      */
     void grow() {
         int depth;
-        Node<T>* otherNode;
+        const Node<T>* otherNode;
 
         Node<T>* newStartNode = _startTree.grow();
         if (newStartNode) {
@@ -157,17 +157,17 @@ class BiRRT {
     int iterationCount() const { return _iterationCount; }
 
 protected:
-    Node<T>* _findBestPath(const T& targetState, Tree<T>& treeToSearch,
-                           int* depthOut) {
-        Node<T>* bestNode = nullptr;
+    const Node<T>* _findBestPath(const T& targetState, Tree<T>& treeToSearch,
+                           int* depthOut) const {
+        const Node<T>* bestNode = nullptr;
         int depth = INT_MAX;
 
-        for (Node<T>* other : treeToSearch.allNodes()) {
+        for (const Node<T> &other : treeToSearch.allNodes()) {
             float dist =
-                _startTree.stateSpace().distance(other->state(), targetState);
-            if (dist < goalMaxDist() && other->depth() < depth) {
-                bestNode = other;
-                depth = other->depth();
+                _startTree.stateSpace().distance(other.state(), targetState);
+            if (dist < goalMaxDist() && other.depth() < depth) {
+                bestNode = &other;
+                depth = other.depth();
             }
         }
 
@@ -183,6 +183,6 @@ class BiRRT {
     int _iterationCount;
 
     int _solutionLength;
-    Node<T> *_startSolutionNode, *_goalSolutionNode;
+    const Node<T> *_startSolutionNode, *_goalSolutionNode;
 };
 };

src/rrt/Tree.hpp

@@ -8,6 +8,7 @@
 #include <rrt/StateSpace.hpp>
 #include <stdexcept>
 #include <vector>
+#include <deque>
 #include <stdlib.h>
 #include <iostream>
 
@@ -21,10 +22,7 @@ namespace RRT {
 template <typename T>
 class Node {
 public:
-    Node(const T& state, Node<T>* parent = nullptr) {
-        _parent = parent;
-        _state = state;
-
+    Node(const T& state, Node<T>* parent = nullptr) : _parent(parent), _state(state) {
         if (_parent) {
             _parent->_children.push_back(this);
         }
@@ -105,6 +103,8 @@ class Node {
 template <typename T>
 class Tree {
 public:
+    Tree(const Tree&) = delete;
+    Tree& operator=(const Tree&) = delete;
     Tree(std::shared_ptr<StateSpace<T>> stateSpace) {
         _stateSpace = stateSpace;
 
@@ -118,8 +118,6 @@ class Tree {
         setGoalMaxDist(0.1);
     }
 
-    virtual ~Tree() { reset(true); }
-
     StateSpace<T>& stateSpace() { return *_stateSpace; }
     const StateSpace<T>& stateSpace() const { return *_stateSpace; }
 
@@ -221,17 +219,11 @@ class Tree {
      * Removes all Nodes from the tree so it can be run() again.
      */
     void reset(bool eraseRoot = false) {
-        if (!_nodes.empty()) {
-            Node<T>* root = _nodes.front();
-            _nodes.erase(_nodes.begin());
-
-            for (Node<T>* n : _nodes) delete n;
+        if (eraseRoot) {
             _nodes.clear();
-
-            if (eraseRoot) {
-                delete root;
-            } else {
-                _nodes.push_back(root);
+        } else {
+            if (!_nodes.empty()) {
+                _nodes.erase(_nodes.begin()+1, _nodes.end());
             }
         }
     }
@@ -266,11 +258,11 @@ class Tree {
         float bestDistance = -1;
         Node<T>* best = nullptr;
 
-        for (Node<T>* other : _nodes) {
-            float dist = _stateSpace->distance(other->state(), state);
+        for (Node<T> &other : _nodes) {
+            float dist = _stateSpace->distance(other.state(), state);
             if (bestDistance < 0 || dist < bestDistance) {
                 bestDistance = dist;
-                best = other;
+                best = &other;
             }
         }
 
@@ -315,9 +307,8 @@ class Tree {
         }
 
         // Add a node to the tree for this state
-        Node<T>* n = new Node<T>(intermediateState, source);
-        _nodes.push_back(n);
-        return n;
+        _nodes.push_back(Node<T>(intermediateState, source));
+        return &_nodes.back();
     }
 
     /**
@@ -327,8 +318,8 @@ class Tree {
      * @param reverse if true, the states will be sent from @dest to the
      *                tree's root
      */
-    void getPath(std::function<void(const T& stateI)> callback, Node<T>* dest,
-                 const bool reverse = false) {
+    void getPath(std::function<void(const T& stateI)> callback, const Node<T>* dest,
+                 const bool reverse = false) const {
         const Node<T>* node = dest;
         if (reverse) {
             while (node) {
@@ -355,34 +346,34 @@ class Tree {
      * @param reverse if true, the states will be sent from @dest to the
      *                tree's root
      */
-    void getPath(std::vector<T>& vectorOut, Node<T>* dest,
-                 const bool reverse = false) {
+    void getPath(std::vector<T>& vectorOut, const Node<T>* dest,
+                 const bool reverse = false) const {
         getPath([&](const T& stateI) { vectorOut.push_back(stateI); }, dest,
                 reverse);
     }
 
     /**
      * @return The root node or nullptr if none exists
      */
-    Node<T>* rootNode() const {
+    const Node<T>* rootNode() const {
         if (_nodes.empty()) return nullptr;
 
-        return _nodes.front();
+        return &_nodes.front();
     }
 
     /**
      * @return The most recent Node added to the tree
      */
-    Node<T>* lastNode() const {
+    const Node<T>* lastNode() const {
         if (_nodes.empty()) return nullptr;
 
-        return _nodes.back();
+        return &_nodes.back();
     }
 
     /**
      * All the nodes
      */
-    const std::vector<Node<T>*> allNodes() const { return _nodes; }
+    const std::deque<Node<T>>& allNodes() const { return _nodes; }
 
     /**
      * @brief The start state for this tree
@@ -397,8 +388,7 @@ class Tree {
         reset(true);
 
         //  create root node from provided start state
-        Node<T>* root = new Node<T>(startState, nullptr);
-        _nodes.push_back(root);
+        _nodes.push_back(Node<T>(startState, nullptr));
     }
 
     /**
@@ -411,7 +401,7 @@ class Tree {
     /**
      * A list of all Node objects in the tree.
      */
-    std::vector<Node<T>*> _nodes;
+    std::deque<Node<T>> _nodes{};
 
     T _goalState;
 
@@ -424,13 +414,13 @@ class Tree {
     /// used for Extended RRTs where growth is biased towards waypoints from
     /// previously grown tree
     float _waypointBias;
-    std::vector<T> _waypoints;
+    std::vector<T> _waypoints{};
 
     float _goalMaxDist;
 
     float _stepSize;
     float _maxStepSize;
 
-    std::shared_ptr<StateSpace<T>> _stateSpace;
+    std::shared_ptr<StateSpace<T>> _stateSpace{};
 };
 }  // namespace RRT