Add templated node

smac_planner/include/smac_planner/a_star.hpp

@@ -24,17 +24,35 @@
 
 #include "smac_planner/node.hpp"
 #include "smac_planner/types.hpp"
+#include "smac_planner/constants.hpp"
 
 namespace smac_planner
 {
 
 /**
  * @class smac_planner::AStarAlgorithm
- * @brief An A* implementation for planning in a costmap
+ * @brief An A* implementation for planning in a costmap. Templated based on the Node type.
  */
+template<typename NodeT>
 class AStarAlgorithm
 {
 public:
+  typedef NodeT * NodePtr;
+  typedef std::vector<NodeT> Graph;
+  typedef std::vector<NodePtr> NodeVector;
+  typedef std::pair<float, NodePtr> NodeElement;
+  typedef std::pair<float, unsigned int> NodeHeuristicPair;
+
+  struct NodeComparator
+  {
+    bool operator()(const NodeElement & a, const NodeElement & b) const
+    {
+      return a.first > b.first;
+    }
+  };
+
+  typedef std::priority_queue<NodeElement, std::vector<NodeElement>, NodeComparator> NodeQueue;
+
   /**
    * @brief A constructor for smac_planner::PlannerServer
    * @param neighborhood The type of neighborhood to use for search (4 or 8 connected)
@@ -99,7 +117,7 @@ class AStarAlgorithm
    * @param path Reference to a vector of indicies of generated path
    * @return whether the path was able to be backtraced
    */
-  bool backtracePath(Node * & node, IndexPath & path);
+  bool backtracePath(NodePtr & node, IndexPath & path);
 
   /**
    * @brief Get maximum number of iterations to plan
@@ -112,60 +130,56 @@ class AStarAlgorithm
    * @brief Get pointer reference to starting node
    * @return Node pointer reference to starting node
    */
-  inline Node * & getStart();
+  inline NodePtr & getStart();
 
   /**
    * @brief Get pointer reference to goal node
    * @return Node pointer reference to goal node
    */
-  inline Node * & getGoal();
+  inline NodePtr & getGoal();
 
   /**
    * @brief Get pointer to next goal in open set
    * @return Node pointer reference to next heuristically scored node
    */
-  inline Node * getNode();
+  inline NodePtr getNode();
 
   /**
    * @brief Get pointer to next goal in open set
    * @param cost The cost to sort into the open set of the node
    * @param node Node pointer reference to add to open set
    */
-  inline void addNode(const float cost, Node * & node);
+  inline void addNode(const float cost, NodePtr & node);
 
   /**
-   * @brief Check if this node is the goal node
-   * @param node Node pointer to check if its the goal node
-   * @return if node is goal
+   * @brief Retrieve all valid neighbors of a node.
+   * @param node Pointer to the node we are currently exploring in A*
+   * @param neighbors Vector of neighbors to be filled
    */
-  inline bool isGoal(Node * & node);
+  inline void getNeighbors(NodePtr & node, NodeVector & neighbors);
 
   /**
-   * @brief Check if this node is valid
-   * @param i Node index
-   * @param node_it Iterator reference of the node
-   * @return whether this node is valid and collision free
+   * @brief Initialize the neighborhood to be used in A*
+   * For now we support 4-connect (VON_NEUMANN) and 8-connect (MOORE)
+   * @param x_size The size of the underlying grid
+   * @param neighborhood The desired neighborhood type
    */
-  inline bool isNodeValid(const unsigned int & i, Node * & node_it);
+  inline void initNeighborhoods(const int & x_size, const Neighborhood & neighborhood);
 
   /**
-   * @brief Get a vector of valid node pointers from relative locations
-   * @param lookup_table Lookup table of values around node to query
-   * @param node Node index
-   * @param neighbors Vector of node pointers to valid node
+   * @brief Check if this node is the goal node
+   * @param node Node pointer to check if its the goal node
+   * @return if node is goal
    */
-  inline void getValidNodes(
-    const std::vector<int> & lookup_table,
-    const int & i,
-    NodeVector & neighbors);
+  inline bool isGoal(NodePtr & node);
 
   /**
    * @brief Get cost of traversal between nodes
-   * @param node Node index current
-   * @param node Node index of new
+   * @param current_node Pointer to current node
+   * @param new_node Pointer to new node
    * @return Reference traversal cost between the nodes
    */
-  inline float getTraversalCost(Node * & lastNode, Node * & node);
+  inline float getTraversalCost(NodePtr & current_node, NodePtr & new_node);
 
   /**
    * @brief Get cost of heuristic of node
@@ -175,13 +189,6 @@ class AStarAlgorithm
    */
   inline float getHeuristicCost(const unsigned int & node);
 
-  /**
-   * @brief Get a vector of neighbors around node
-   * @param node Node index
-   * @param neighbors Vector of node pointers to neighbors
-   */
-  inline void getNeighbors(const unsigned int & node, NodeVector & neighbors);
-
   /**
    * @brief Check if inputs to planner are valid
    * @return Are valid
@@ -234,16 +241,14 @@ class AStarAlgorithm
   unsigned int _y_size;
 
   Coordinates _goal_coordinates;
-  Node * _start;
-  Node * _goal;
+  NodePtr _start;
+  NodePtr _goal;
 
   std::unique_ptr<Graph> _graph;
   std::unique_ptr<NodeQueue> _queue;
 
   Neighborhood _neighborhood;
-  std::vector<int> _van_neumann_neighborhood;
-  std::vector<int> _moore_neighborhood;
-
+  std::vector<int> _neighbors_grid_offsets;
   NodeHeuristicPair _best_heuristic_node;
 };
 

smac_planner/include/smac_planner/constants.hpp

@@ -0,0 +1,47 @@
+// Copyright (c) 2020, Samsung Research America
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License. Reserved.
+
+#ifndef SMAC_PLANNER__CONSTANTS_HPP_
+#define SMAC_PLANNER__CONSTANTS_HPP_
+
+namespace smac_planner
+{
+  enum class Neighborhood
+  {
+    UNKNOWN = 0,
+    VON_NEUMANN = 1,
+    MOORE = 2
+  };
+
+  inline std::string toString(const Neighborhood & n)
+  {
+    switch (n) {
+      case Neighborhood::VON_NEUMANN:
+        return "Von Neumann";
+      case Neighborhood::MOORE:
+        return "Moore";
+      default:
+        return "Unknown";
+    }
+  }
+
+  const float UNKNOWN = 255;
+  const float OCCUPIED = 254;
+  const float INSCRIBED = 253;
+  const float MAX_NON_OBSTACLE = 252;
+  const float FREE = 0;
+
+}  // namespace smac_planner
+
+#endif  // SMAC_PLANNER__CONSTANTS_HPP_

smac_planner/include/smac_planner/node.hpp

@@ -20,6 +20,8 @@
 #include <queue>
 #include <limits>
 
+#include "smac_planner/constants.hpp"
+
 namespace smac_planner
 {
 
@@ -36,10 +38,10 @@ class Node
    * @param index The index of this node for self-reference
    */
   explicit Node(unsigned char & cost_in, const unsigned int index)
-  : last_node(nullptr),
+  : parent(nullptr),
     _cell_cost(static_cast<float>(cost_in)),
     _accumulated_cost(std::numeric_limits<float>::max()),
-    _i(index),
+    _index(index),
     _was_visited(false),
     _is_queued(false)
   {
@@ -50,7 +52,7 @@ class Node
    */
   ~Node()
   {
-    last_node = nullptr;
+    parent = nullptr;
   }
 
   /**
@@ -60,7 +62,7 @@ class Node
    */
   bool operator==(const Node & rhs)
   {
-    return this->_i == rhs._i;
+    return this->_index == rhs._index;
   }
 
   /**
@@ -70,10 +72,10 @@ class Node
    */
   void reset(const unsigned char & cost, const unsigned int index)
   {
-    last_node = nullptr;
+    parent = nullptr;
     _cell_cost = static_cast<float>(cost);
     _accumulated_cost = std::numeric_limits<float>::max();
-    _i = index;
+    _index = index;
     _was_visited = false;
     _is_queued = false;
   }
@@ -146,15 +148,43 @@ class Node
    */
   unsigned int & getIndex()
   {
-    return _i;
+    return _index;
+  }
+
+  /**
+   * @brief Check if this node is valid
+   * @param traverse_unknown If we can explore unknown nodes on the graph
+   * @return whether this node is valid and collision free
+   */
+  bool isNodeValid(const bool & traverse_unknown) {
+    // NOTE(stevemacenski): Right now, we do not check if the node has wrapped around
+    // the regular grid (e.g. your node is on the edge of the costmap and i+1
+    // goes to the other side). This check would add compute time and my assertion is
+    // that if you do wrap around, the heuristic will be so high it'll be added far
+    // in the queue that it will never be called if a valid path exists.
+    // This is intentionally un-included to increase speed, but be aware. If this causes
+    // trouble, please file a ticket and we can address it then.
+
+    // occupied node
+    auto & cost = this->getCost();
+    if (cost == OCCUPIED || cost == INSCRIBED) {
+      return false;
+    }
+
+    // unknown node
+    if (cost == UNKNOWN && ! traverse_unknown) {
+      return false;
+    }
+
+    return true;
   }
 
-  Node * last_node;
+  Node * parent;
 
 private:
   float _cell_cost;
   float _accumulated_cost;
-  unsigned int _i;
+  unsigned int _index;
   bool _was_visited;
   bool _is_queued;
 };

smac_planner/include/smac_planner/smac_planner.hpp

@@ -89,7 +89,7 @@ class SmacPlanner : public nav2_core::GlobalPlanner
   void removeHook(std::vector<Eigen::Vector2d> & path);
 
 protected:
-  std::unique_ptr<AStarAlgorithm> _a_star;
+  std::unique_ptr<AStarAlgorithm<Node>> _a_star;
   std::unique_ptr<Smoother> _smoother;
   std::unique_ptr<Upsampler> _upsampler;
   nav2_util::LifecycleNode::SharedPtr _node;

smac_planner/include/smac_planner/types.hpp

@@ -16,64 +16,15 @@
 #define SMAC_PLANNER__TYPES_HPP_
 
 #include <vector>
-#include <unordered_map>
-#include <utility>
-#include <queue>
-
-#include "smac_planner/node.hpp"
 
 namespace smac_planner
 {
-
-typedef std::vector<Node> Graph;
-
-typedef std::vector<Node *> NodeVector;
-
 typedef std::vector<unsigned int> IndexPath;
 
 typedef std::pair<float, float> Coordinates;
 
 typedef std::pair<double, double> DoubleCoordinates;
 
-typedef std::pair<float, Node *> NodeElement;
-
-typedef std::pair<float, unsigned int> NodeHeuristicPair;
-
-struct NodeComparator
-{
-  bool operator()(const NodeElement & a, const NodeElement & b) const
-  {
-    return a.first > b.first;
-  }
-};
-
-typedef std::priority_queue<NodeElement, std::vector<NodeElement>, NodeComparator> NodeQueue;
-
-enum class Neighborhood
-{
-  UNKNOWN = 0,
-  VAN_NEUMANN = 1,
-  MOORE = 2
-};
-
-inline std::string toString(const Neighborhood & n)
-{
-  switch (n) {
-    case Neighborhood::VAN_NEUMANN:
-      return "Van Neumann";
-    case Neighborhood::MOORE:
-      return "Moore";
-    default:
-      return "Unknown";
-  }
-}
-
-const float UNKNOWN = 255;
-const float OCCUPIED = 254;
-const float INSCRIBED = 253;
-const float MAX_NON_OBSTACLE = 252;
-const float FREE = 0;
-
 }  // namespace smac_planner
 
 #endif  // SMAC_PLANNER__TYPES_HPP_