[Smac] Robin hood data structure improves performance by 10-15%! (#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
ros-navigation · Nov 8, 2022 · 46f6b19 · 46f6b19
1 parent 2a42a1b
commit 46f6b19
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;