Continuation of ros-navigation#4284 (ros-navigation#4295)

* 64 bit for index

Signed-off-by: Guillaume Doisy <guillaume@dexory.com>

* Graph storing in uint64

* Remove incorrect usage

---------

Signed-off-by: Guillaume Doisy <guillaume@dexory.com>
Co-authored-by: Guillaume Doisy <guillaume@dexory.com>

nav2_smac_planner/include/nav2_smac_planner/a_star.hpp

@@ -49,13 +49,13 @@ class AStarAlgorithm
 {
 public:
   typedef NodeT * NodePtr;
-  typedef robin_hood::unordered_node_map<unsigned int, NodeT> Graph;
+  typedef robin_hood::unordered_node_map<uint64_t, NodeT> Graph;
   typedef std::vector<NodePtr> NodeVector;
   typedef std::pair<float, NodeBasic<NodeT>> NodeElement;
   typedef typename NodeT::Coordinates Coordinates;
   typedef typename NodeT::CoordinateVector CoordinateVector;
   typedef typename NodeVector::iterator NeighborIterator;
-  typedef std::function<bool (const unsigned int &, NodeT * &)> NodeGetter;
+  typedef std::function<bool (const uint64_t &, NodeT * &)> NodeGetter;
 
   /**
    * @struct nav2_smac_planner::NodeComparator
@@ -210,7 +210,7 @@ class AStarAlgorithm
    * @brief Adds node to graph
    * @param index Node index to add
    */
-  inline NodePtr addToGraph(const unsigned int & index);
+  inline NodePtr addToGraph(const uint64_t & index);
 
   /**
    * @brief Check if this node is the goal node

nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp

@@ -35,7 +35,7 @@ class AnalyticExpansion
 public:
   typedef NodeT * NodePtr;
   typedef typename NodeT::Coordinates Coordinates;
-  typedef std::function<bool (const unsigned int &, NodeT * &)> NodeGetter;
+  typedef std::function<bool (const uint64_t &, NodeT * &)> NodeGetter;
 
   /**
    * @struct nav2_smac_planner::AnalyticExpansion::AnalyticExpansionNodes

nav2_smac_planner/include/nav2_smac_planner/node_2d.hpp

@@ -62,7 +62,7 @@ class Node2D
    * @brief A constructor for nav2_smac_planner::Node2D
    * @param index The index of this node for self-reference
    */
-  explicit Node2D(const unsigned int index);
+  explicit Node2D(const uint64_t index);
 
   /**
    * @brief A destructor for nav2_smac_planner::Node2D
@@ -158,7 +158,7 @@ class Node2D
    * @brief Gets cell index
    * @return Reference to cell index
    */
-  inline unsigned int getIndex()
+  inline uint64_t getIndex()
   {
     return _index;
   }
@@ -185,10 +185,11 @@ class Node2D
    * @param width width of costmap
    * @return index
    */
-  static inline unsigned int getIndex(
+  static inline uint64_t getIndex(
     const unsigned int & x, const unsigned int & y, const unsigned int & width)
   {
-    return x + y * width;
+    return static_cast<uint64_t>(x) + static_cast<uint64_t>(y) *
+           static_cast<uint64_t>(width);
   }
 
   /**
@@ -199,7 +200,7 @@ class Node2D
    * @return coordinates of point
    */
   static inline Coordinates getCoords(
-    const unsigned int & index, const unsigned int & width, const unsigned int & angles)
+    const uint64_t & index, const unsigned int & width, const unsigned int & angles)
   {
     if (angles != 1) {
       throw std::runtime_error("Node type Node2D does not have a valid angle quantization.");
@@ -213,7 +214,7 @@ class Node2D
    * @param Index Index of point
    * @return coordinates of point
    */
-  static inline Coordinates getCoords(const unsigned int & index)
+  static inline Coordinates getCoords(const uint64_t & index)
   {
     const unsigned int & size_x = _neighbors_grid_offsets[3];
     return Coordinates(index % size_x, index / size_x);
@@ -253,7 +254,8 @@ class Node2D
    * @param neighbors Vector of neighbors to be filled
    */
   void getNeighbors(
-    std::function<bool(const unsigned int &, nav2_smac_planner::Node2D * &)> & validity_checker,
+    std::function<bool(const uint64_t &,
+    nav2_smac_planner::Node2D * &)> & validity_checker,
     GridCollisionChecker * collision_checker,
     const bool & traverse_unknown,
     NodeVector & neighbors);
@@ -272,7 +274,7 @@ class Node2D
 private:
   float _cell_cost;
   float _accumulated_cost;
-  unsigned int _index;
+  uint64_t _index;
   bool _was_visited;
   bool _is_queued;
 };

nav2_smac_planner/include/nav2_smac_planner/node_basic.hpp

@@ -49,7 +49,7 @@ class NodeBasic
    * @brief A constructor for nav2_smac_planner::NodeBasic
    * @param index The index of this node for self-reference
    */
-  explicit NodeBasic(const unsigned int index)
+  explicit NodeBasic(const uint64_t index)
   : index(index),
     graph_node_ptr(nullptr)
   {
@@ -73,7 +73,8 @@ class NodeBasic
   typename NodeT::Coordinates pose;  // Used by NodeHybrid and NodeLattice
   NodeT * graph_node_ptr;
   MotionPrimitive * prim_ptr;  // Used by NodeLattice
-  unsigned int index, motion_index;
+  uint64_t index;
+  unsigned int motion_index;
   bool backward;
   TurnDirection turn_dir;
 };

nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp

@@ -40,7 +40,7 @@ namespace nav2_smac_planner
 typedef std::vector<float> LookupTable;
 typedef std::pair<double, double> TrigValues;
 
-typedef std::pair<float, unsigned int> ObstacleHeuristicElement;
+typedef std::pair<float, uint64_t> ObstacleHeuristicElement;
 struct ObstacleHeuristicComparator
 {
   bool operator()(const ObstacleHeuristicElement & a, const ObstacleHeuristicElement & b) const
@@ -184,7 +184,7 @@ class NodeHybrid
    * @brief A constructor for nav2_smac_planner::NodeHybrid
    * @param index The index of this node for self-reference
    */
-  explicit NodeHybrid(const unsigned int index);
+  explicit NodeHybrid(const uint64_t index);
 
   /**
    * @brief A destructor for nav2_smac_planner::NodeHybrid
@@ -291,7 +291,7 @@ class NodeHybrid
    * @brief Gets cell index
    * @return Reference to cell index
    */
-  inline unsigned int getIndex()
+  inline uint64_t getIndex()
   {
     return _index;
   }
@@ -319,11 +319,14 @@ class NodeHybrid
    * @param angle_quantization Number of theta bins
    * @return Index
    */
-  static inline unsigned int getIndex(
+  static inline uint64_t getIndex(
     const unsigned int & x, const unsigned int & y, const unsigned int & angle,
     const unsigned int & width, const unsigned int & angle_quantization)
   {
-    return angle + x * angle_quantization + y * width * angle_quantization;
+    return static_cast<uint64_t>(angle) + static_cast<uint64_t>(x) *
+           static_cast<uint64_t>(angle_quantization) +
+           static_cast<uint64_t>(y) * static_cast<uint64_t>(width) *
+           static_cast<uint64_t>(angle_quantization);
   }
 
   /**
@@ -333,7 +336,7 @@ class NodeHybrid
    * @param angle Theta coordinate of point
    * @return Index
    */
-  static inline unsigned int getIndex(
+  static inline uint64_t getIndex(
     const unsigned int & x, const unsigned int & y, const unsigned int & angle)
   {
     return getIndex(
@@ -349,7 +352,7 @@ class NodeHybrid
    * @return Coordinates
    */
   static inline Coordinates getCoords(
-    const unsigned int & index,
+    const uint64_t & index,
     const unsigned int & width, const unsigned int & angle_quantization)
   {
     return Coordinates(
@@ -447,7 +450,8 @@ class NodeHybrid
    * @param neighbors Vector of neighbors to be filled
    */
   void getNeighbors(
-    std::function<bool(const unsigned int &, nav2_smac_planner::NodeHybrid * &)> & validity_checker,
+    std::function<bool(const uint64_t &,
+    nav2_smac_planner::NodeHybrid * &)> & validity_checker,
     GridCollisionChecker * collision_checker,
     const bool & traverse_unknown,
     NodeVector & neighbors);
@@ -478,7 +482,7 @@ class NodeHybrid
 private:
   float _cell_cost;
   float _accumulated_cost;
-  unsigned int _index;
+  uint64_t _index;
   bool _was_visited;
   unsigned int _motion_primitive_index;
   TurnDirection _turn_dir;

nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp

@@ -132,7 +132,7 @@ class NodeLattice
    * @brief A constructor for nav2_smac_planner::NodeLattice
    * @param index The index of this node for self-reference
    */
-  explicit NodeLattice(const unsigned int index);
+  explicit NodeLattice(const uint64_t index);
 
   /**
    * @brief A destructor for nav2_smac_planner::NodeLattice
@@ -229,7 +229,7 @@ class NodeLattice
    * @brief Gets cell index
    * @return Reference to cell index
    */
-  inline unsigned int getIndex()
+  inline uint64_t getIndex()
   {
     return _index;
   }
@@ -281,7 +281,7 @@ class NodeLattice
    * @param angle Theta coordinate of point
    * @return Index
    */
-  static inline unsigned int getIndex(
+  static inline uint64_t getIndex(
     const unsigned int & x, const unsigned int & y, const unsigned int & angle)
   {
     // Hybrid-A* and State Lattice share a coordinate system
@@ -298,7 +298,7 @@ class NodeLattice
    * @return Coordinates
    */
   static inline Coordinates getCoords(
-    const unsigned int & index,
+    const uint64_t & index,
     const unsigned int & width, const unsigned int & angle_quantization)
   {
     // Hybrid-A* and State Lattice share a coordinate system
@@ -396,7 +396,7 @@ class NodeLattice
    * @param neighbors Vector of neighbors to be filled
    */
   void getNeighbors(
-    std::function<bool(const unsigned int &,
+    std::function<bool(const uint64_t &,
     nav2_smac_planner::NodeLattice * &)> & validity_checker,
     GridCollisionChecker * collision_checker,
     const bool & traverse_unknown,
@@ -425,7 +425,7 @@ class NodeLattice
 private:
   float _cell_cost;
   float _accumulated_cost;
-  unsigned int _index;
+  uint64_t _index;
   bool _was_visited;
   MotionPrimitive * _motion_primitive;
   bool _backwards;

nav2_smac_planner/include/nav2_smac_planner/types.hpp

@@ -26,7 +26,7 @@
 namespace nav2_smac_planner
 {
 
-typedef std::pair<float, unsigned int> NodeHeuristicPair;
+typedef std::pair<float, uint64_t> NodeHeuristicPair;
 
 /**
  * @struct nav2_smac_planner::SearchInfo

nav2_smac_planner/src/a_star.cpp

@@ -120,7 +120,7 @@ void AStarAlgorithm<NodeT>::setCollisionChecker(GridCollisionChecker * collision
 
 template<typename NodeT>
 typename AStarAlgorithm<NodeT>::NodePtr AStarAlgorithm<NodeT>::addToGraph(
-  const unsigned int & index)
+  const uint64_t & index)
 {
   auto iter = _graph.find(index);
   if (iter != _graph.end()) {
@@ -287,9 +287,11 @@ bool AStarAlgorithm<NodeT>::createPath(
   int closest_distance = std::numeric_limits<int>::max();
 
   // Given an index, return a node ptr reference if its collision-free and valid
-  const unsigned int max_index = getSizeX() * getSizeY() * getSizeDim3();
+  const uint64_t max_index = static_cast<uint64_t>(getSizeX()) *
+    static_cast<uint64_t>(getSizeY()) *
+    static_cast<uint64_t>(getSizeDim3());
   NodeGetter neighborGetter =
-    [&, this](const unsigned int & index, NodePtr & neighbor_rtn) -> bool
+    [&, this](const uint64_t & index, NodePtr & neighbor_rtn) -> bool
     {
       if (index >= max_index) {
         return false;

nav2_smac_planner/src/analytic_expansion.cpp

@@ -201,7 +201,7 @@ typename AnalyticExpansion<NodeT>::AnalyticExpansionNodes AnalyticExpansion<Node
 
   // Pre-allocate
   NodePtr prev(node);
-  unsigned int index = 0;
+  uint64_t index = 0;
   NodePtr next(nullptr);
   float angle = 0.0;
   Coordinates proposed_coordinates;

nav2_smac_planner/src/node_2d.cpp

@@ -25,7 +25,7 @@ namespace nav2_smac_planner
 std::vector<int> Node2D::_neighbors_grid_offsets;
 float Node2D::cost_travel_multiplier = 2.0;
 
-Node2D::Node2D(const unsigned int index)
+Node2D::Node2D(const uint64_t index)
 : parent(nullptr),
   _cell_cost(std::numeric_limits<float>::quiet_NaN()),
   _accumulated_cost(std::numeric_limits<float>::max()),
@@ -108,7 +108,8 @@ void Node2D::initMotionModel(
 }
 
 void Node2D::getNeighbors(
-  std::function<bool(const unsigned int &, nav2_smac_planner::Node2D * &)> & NeighborGetter,
+  std::function<bool(const uint64_t &,
+  nav2_smac_planner::Node2D * &)> & NeighborGetter,
   GridCollisionChecker * collision_checker,
   const bool & traverse_unknown,
   NodeVector & neighbors)
@@ -124,9 +125,9 @@ void Node2D::getNeighbors(
   // 100 100 100   where lower-middle '100' is visited with same cost by both bottom '50' nodes
   // Therefore, it is valuable to have some low-potential across the entire map
   // rather than a small inflation around the obstacles
-  int index;
+  uint64_t index;
   NodePtr neighbor;
-  int node_i = this->getIndex();
+  uint64_t node_i = this->getIndex();
   const Coordinates parent = getCoords(this->getIndex());
   Coordinates child;
 

nav2_smac_planner/src/node_hybrid.cpp

@@ -338,7 +338,7 @@ float HybridMotionTable::getAngleFromBin(const unsigned int & bin_idx)
   return bin_idx * bin_size;
 }
 
-NodeHybrid::NodeHybrid(const unsigned int index)
+NodeHybrid::NodeHybrid(const uint64_t index)
 : parent(nullptr),
   pose(0.0f, 0.0f, 0.0f),
   _cell_cost(std::numeric_limits<float>::quiet_NaN()),
@@ -468,7 +468,7 @@ void NodeHybrid::initMotionModel(
 }
 
 inline float distanceHeuristic2D(
-  const unsigned int idx, const unsigned int size_x,
+  const uint64_t idx, const unsigned int size_x,
   const unsigned int target_x, const unsigned int target_y)
 {
   int dx = static_cast<int>(idx % size_x) - static_cast<int>(target_x);
@@ -611,8 +611,8 @@ float NodeHybrid::getObstacleHeuristic(
   const int size_x_int = static_cast<int>(size_x);
   const float sqrt2 = sqrtf(2.0f);
   float c_cost, cost, travel_cost, new_cost, existing_cost;
-  unsigned int idx, mx, my;
-  unsigned int new_idx = 0;
+  unsigned int mx, my;
+  unsigned int idx, new_idx = 0;
 
   const std::vector<int> neighborhood = {1, -1,  // left right
     size_x_int, -size_x_int,  // up down
@@ -855,12 +855,13 @@ void NodeHybrid::precomputeDistanceHeuristic(
 }
 
 void NodeHybrid::getNeighbors(
-  std::function<bool(const unsigned int &, nav2_smac_planner::NodeHybrid * &)> & NeighborGetter,
+  std::function<bool(const uint64_t &,
+  nav2_smac_planner::NodeHybrid * &)> & NeighborGetter,
   GridCollisionChecker * collision_checker,
   const bool & traverse_unknown,
   NodeVector & neighbors)
 {
-  unsigned int index = 0;
+  uint64_t index = 0;
   NodePtr neighbor = nullptr;
   Coordinates initial_node_coords;
   const MotionPoses motion_projections = motion_table.getProjections(this);

nav2_smac_planner/src/node_lattice.cpp

@@ -180,7 +180,7 @@ float & LatticeMotionTable::getAngleFromBin(const unsigned int & bin_idx)
   return lattice_metadata.heading_angles[bin_idx];
 }
 
-NodeLattice::NodeLattice(const unsigned int index)
+NodeLattice::NodeLattice(const uint64_t index)
 : parent(nullptr),
   pose(0.0f, 0.0f, 0.0f),
   _cell_cost(std::numeric_limits<float>::quiet_NaN()),
@@ -469,12 +469,13 @@ void NodeLattice::precomputeDistanceHeuristic(
 }
 
 void NodeLattice::getNeighbors(
-  std::function<bool(const unsigned int &, nav2_smac_planner::NodeLattice * &)> & NeighborGetter,
+  std::function<bool(const uint64_t &,
+  nav2_smac_planner::NodeLattice * &)> & NeighborGetter,
   GridCollisionChecker * collision_checker,
   const bool & traverse_unknown,
   NodeVector & neighbors)
 {
-  unsigned int index = 0;
+  uint64_t index = 0;
   float angle;
   bool backwards = false;
   NodePtr neighbor = nullptr;