[Smac] Robin hood data structure improves performance by 10-15%! (ros…

…-navigation#3201) * adding robin_hood unordered_map * using robin_hood node map * ignore robin_hood file * linting * linting cont. for triple pointers * linting cont. for uncrustify
jwallace42 · Dec 14, 2022 · 5ae4b73 · 5ae4b73
1 parent 278f896
commit 5ae4b73
Show file tree

Hide file tree

Showing 5 changed files with 2,544 additions and 3 deletions.
diff --git a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py
@@ -331,7 +331,7 @@ def _getPathImpl(self, start, goal, planner_id='', use_start=False):
 
  def getPath(self, start, goal, planner_id='', use_start=False):
  """Send a `ComputePathToPose` action request."""
- rtn = _getPathImpl(start, goal, planner_id='', use_start=False)
+ rtn = self._getPathImpl(start, goal, planner_id='', use_start=False)
  if not rtn:
  return None
  else:

diff --git a/nav2_smac_planner/CMakeLists.txt b/nav2_smac_planner/CMakeLists.txt
@@ -152,6 +152,7 @@ install(DIRECTORY lattice_primitives/sample_primitives DESTINATION share/${PROJE
 
 if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
+ set(AMENT_LINT_AUTO_FILE_EXCLUDE include/nav2_smac_planner/thirdparty/robin_hood.h)
  ament_lint_auto_find_test_dependencies()
  find_package(ament_cmake_gtest REQUIRED)
  add_subdirectory(test)

diff --git a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp
@@ -26,6 +26,7 @@
 
 #include "nav2_costmap_2d/costmap_2d.hpp"
 
+#include "nav2_smac_planner/thirdparty/robin_hood.h"
 #include "nav2_smac_planner/analytic_expansion.hpp"
 #include "nav2_smac_planner/node_2d.hpp"
 #include "nav2_smac_planner/node_hybrid.hpp"
@@ -46,7 +47,7 @@ class AStarAlgorithm
 {
 public:
  typedef NodeT * NodePtr;
- typedef std::unordered_map<unsigned int, NodeT> Graph;
+ typedef robin_hood::unordered_node_map<unsigned int, NodeT> Graph;
  typedef std::vector<NodePtr> NodeVector;
  typedef std::pair<float, NodeBasic<NodeT>> NodeElement;
  typedef typename NodeT::Coordinates Coordinates;