Smac Planner Optimizations; 9.1% improvement in speed (ros-navigation…

…#4257) * optimizations * Update test_nodehybrid.cpp Signed-off-by: Steve Macenski <[email protected]> --------- Signed-off-by: Steve Macenski <[email protected]>
omarwalid96 · Apr 11, 2024 · c097088 · c097088
1 parent 61a774b
commit c097088
Show file tree

Hide file tree

Showing 7 changed files with 59 additions and 41 deletions.
diff --git a/nav2_smac_planner/CMakeLists.txt b/nav2_smac_planner/CMakeLists.txt
@@ -23,14 +23,11 @@ find_package(eigen3_cmake_module REQUIRED)
 find_package(Eigen3 REQUIRED)
 find_package(angles REQUIRED)
 find_package(ompl REQUIRED)
-find_package(OpenMP REQUIRED)
 
 if(NOT CMAKE_CXX_STANDARD)
   set(CMAKE_CXX_STANDARD 17)
 endif()
 
-set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
-
 if(MSVC)
   add_compile_definitions(_USE_MATH_DEFINES)
 else()
@@ -40,16 +37,8 @@ endif()
 include_directories(
   include
   ${OMPL_INCLUDE_DIRS}
-  ${OpenMP_INCLUDE_DIRS}
 )
 
-find_package(OpenMP)
-if(OPENMP_FOUND)
-    set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
-    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
-    set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}")
-endif()
-
 set(library_name nav2_smac_planner)
 
 set(dependencies
@@ -85,7 +74,7 @@ add_library(${library_name} SHARED
   src/node_basic.cpp
 )
 
-target_link_libraries(${library_name} ${OMPL_LIBRARIES} ${OpenMP_LIBRARIES}  OpenMP::OpenMP_CXX)
+target_link_libraries(${library_name} ${OMPL_LIBRARIES})
 target_include_directories(${library_name} PUBLIC ${Eigen3_INCLUDE_DIRS})
 
 ament_target_dependencies(${library_name}

diff --git a/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp b/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp
@@ -463,16 +463,14 @@ class NodeHybrid
   Coordinates pose;
 
   // Constants required across all nodes but don't want to allocate more than once
-  static double travel_distance_cost;
+  static float travel_distance_cost;
   static HybridMotionTable motion_table;
   // Wavefront lookup and queue for continuing to expand as needed
   static LookupTable obstacle_heuristic_lookup_table;
   static ObstacleHeuristicQueue obstacle_heuristic_queue;
 
-  static nav2_costmap_2d::Costmap2D * sampled_costmap;
   static std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros;
   static std::shared_ptr<nav2_costmap_2d::InflationLayer> inflation_layer;
-  static CostmapDownsampler downsampler;
   // Dubin / Reeds-Shepp lookup and size for dereferencing
   static LookupTable dist_heuristic_lookup_table;
   static float size_lookup;

diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp
@@ -365,7 +365,7 @@ bool AStarAlgorithm<NodeT>::createPath(
   }
 
   if (_best_heuristic_node.first < getToleranceHeuristic()) {
-    // If we run out of serach options, return the path that is closest, if within tolerance.
+    // If we run out of search options, return the path that is closest, if within tolerance.
     return _graph.at(_best_heuristic_node.second).backtracePath(path);
   }
 

diff --git a/nav2_smac_planner/src/analytic_expansion.cpp b/nav2_smac_planner/src/analytic_expansion.cpp
@@ -180,7 +180,7 @@ typename AnalyticExpansion<NodeT>::AnalyticExpansionNodes AnalyticExpansion<Node
   float d = state_space->distance(from(), to());
 
   // A move of sqrt(2) is guaranteed to be in a new cell
-  static const float sqrt_2 = std::sqrt(2.0f);
+  static const float sqrt_2 = sqrtf(2.0f);
 
   // If the length is too far, exit. This prevents unsafe shortcutting of paths
   // into higher cost areas far out from the goal itself, let search to the work of getting

diff --git a/nav2_smac_planner/src/collision_checker.cpp b/nav2_smac_planner/src/collision_checker.cpp
@@ -112,10 +112,10 @@ bool GridCollisionChecker::inCollision(
     // if footprint, then we check for the footprint's points, but first see
     // if the robot is even potentially in an inscribed collision
     footprint_cost_ = static_cast<float>(costmap_->getCost(
-        static_cast<unsigned int>(x + 0.5), static_cast<unsigned int>(y + 0.5)));
+        static_cast<unsigned int>(x + 0.5f), static_cast<unsigned int>(y + 0.5f)));
 
     if (footprint_cost_ < possible_collision_cost_) {
-      if (possible_collision_cost_ > 0) {
+      if (possible_collision_cost_ > 0.0f) {
         return false;
       } else {
         RCLCPP_ERROR_THROTTLE(
@@ -162,7 +162,7 @@ bool GridCollisionChecker::inCollision(
   } else {
     // if radius, then we can check the center of the cost assuming inflation is used
     footprint_cost_ = static_cast<float>(costmap_->getCost(
-        static_cast<unsigned int>(x + 0.5), static_cast<unsigned int>(y + 0.5)));
+        static_cast<unsigned int>(x + 0.5f), static_cast<unsigned int>(y + 0.5f)));
 
     if (footprint_cost_ == UNKNOWN && traverse_unknown) {
       return false;

diff --git a/nav2_smac_planner/src/node_hybrid.cpp b/nav2_smac_planner/src/node_hybrid.cpp
@@ -36,15 +36,13 @@ namespace nav2_smac_planner
 
 // defining static member for all instance to share
 LookupTable NodeHybrid::obstacle_heuristic_lookup_table;
-double NodeHybrid::travel_distance_cost = sqrt(2);
+float NodeHybrid::travel_distance_cost = sqrtf(2.0f);
 HybridMotionTable NodeHybrid::motion_table;
 float NodeHybrid::size_lookup = 25;
 LookupTable NodeHybrid::dist_heuristic_lookup_table;
-nav2_costmap_2d::Costmap2D * NodeHybrid::sampled_costmap = nullptr;
 std::shared_ptr<nav2_costmap_2d::Costmap2DROS> NodeHybrid::costmap_ros = nullptr;
 std::shared_ptr<nav2_costmap_2d::InflationLayer> NodeHybrid::inflation_layer = nullptr;
 
-CostmapDownsampler NodeHybrid::downsampler;
 ObstacleHeuristicQueue NodeHybrid::obstacle_heuristic_queue;
 
 // Each of these tables are the projected motion models through
@@ -489,35 +487,37 @@ void NodeHybrid::resetObstacleHeuristic(
   // than 0.05 * normalized cost. Since this is just a search prior, there's no loss in generality
   costmap_ros = costmap_ros_i;
   inflation_layer = nav2_costmap_2d::InflationLayer::getInflationLayer(costmap_ros);
-  sampled_costmap = costmap_ros->getCostmap();
+  auto costmap = costmap_ros->getCostmap();
+
+  // Clear lookup table
+  unsigned int size = 0u;
+  unsigned int size_x = 0u;
   if (motion_table.downsample_obstacle_heuristic) {
-    std::weak_ptr<nav2_util::LifecycleNode> ptr;
-    downsampler.on_configure(ptr, "fake_frame", "fake_topic", sampled_costmap, 2.0, true);
-    downsampler.on_activate();
-    sampled_costmap = downsampler.downsample(2.0);
+    size_x = ceil(static_cast<float>(costmap->getSizeInCellsX()) / 2.0f);
+    size = size_x *
+      ceil(static_cast<float>(costmap->getSizeInCellsY()) / 2.0f);
+  } else {
+    size_x = costmap->getSizeInCellsX();
+    size = size_x * costmap->getSizeInCellsY();
   }
 
-  // Clear lookup table
-  unsigned int size = sampled_costmap->getSizeInCellsX() * sampled_costmap->getSizeInCellsY();
   if (obstacle_heuristic_lookup_table.size() == size) {
     // must reset all values
     std::fill(
       obstacle_heuristic_lookup_table.begin(),
-      obstacle_heuristic_lookup_table.end(), 0.0);
+      obstacle_heuristic_lookup_table.end(), 0.0f);
   } else {
     unsigned int obstacle_size = obstacle_heuristic_lookup_table.size();
-    obstacle_heuristic_lookup_table.resize(size, 0.0);
+    obstacle_heuristic_lookup_table.resize(size, 0.0f);
     // must reset values for non-constructed indices
     std::fill_n(
-      obstacle_heuristic_lookup_table.begin(), obstacle_size, 0.0);
+      obstacle_heuristic_lookup_table.begin(), obstacle_size, 0.0f);
   }
 
   obstacle_heuristic_queue.clear();
-  obstacle_heuristic_queue.reserve(
-    sampled_costmap->getSizeInCellsX() * sampled_costmap->getSizeInCellsY());
+  obstacle_heuristic_queue.reserve(size);
 
   // Set initial goal point to queue from. Divided by 2 due to downsampled costmap.
-  const unsigned int size_x = sampled_costmap->getSizeInCellsX();
   unsigned int goal_index;
   if (motion_table.downsample_obstacle_heuristic) {
     goal_index = floor(goal_y / 2.0f) * size_x + floor(goal_x / 2.0f);
@@ -562,8 +562,17 @@ float NodeHybrid::getObstacleHeuristic(
   const float & cost_penalty)
 {
   // If already expanded, return the cost
-  const unsigned int size_x = sampled_costmap->getSizeInCellsX();
+  auto costmap = costmap_ros->getCostmap();
   const bool is_circular = costmap_ros->getUseRadius();
+  unsigned int size_x = 0u;
+  unsigned int size_y = 0u;
+  if (motion_table.downsample_obstacle_heuristic) {
+    size_x = ceil(static_cast<float>(costmap->getSizeInCellsX()) / 2.0f);
+    size_y = ceil(static_cast<float>(costmap->getSizeInCellsY()) / 2.0f);
+  } else {
+    size_x = costmap->getSizeInCellsX();
+    size_y = costmap->getSizeInCellsY();
+  }
 
   // Divided by 2 due to downsampled costmap.
   unsigned int start_y, start_x;
@@ -600,8 +609,7 @@ float NodeHybrid::getObstacleHeuristic(
     ObstacleHeuristicComparator{});
 
   const int size_x_int = static_cast<int>(size_x);
-  const unsigned int size_y = sampled_costmap->getSizeInCellsY();
-  const float sqrt2 = sqrt(2.0f);
+  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;
@@ -632,7 +640,30 @@ float NodeHybrid::getObstacleHeuristic(
 
       // if neighbor path is better and non-lethal, set new cost and add to queue
       if (new_idx < size_x * size_y) {
-        cost = static_cast<float>(sampled_costmap->getCost(new_idx));
+        if (downsample_H) {
+          // Get costmap values as if downsampled
+          unsigned int y_offset = (new_idx / size_x) * 2;
+          unsigned int x_offset = (new_idx - ((new_idx / size_x) * size_x)) * 2;
+          cost = costmap->getCost(x_offset, y_offset);
+          for (unsigned int i = 0; i < 2u; ++i) {
+            unsigned int mxd = x_offset + i;
+            if (mxd >= costmap->getSizeInCellsX()) {
+              continue;
+            }
+            for (unsigned int j = 0; j < 2u; ++j) {
+              unsigned int myd = y_offset + j;
+              if (myd >= costmap->getSizeInCellsY()) {
+                continue;
+              }
+              if (i == 0 && j == 0) {
+                continue;
+              }
+              cost = std::min(cost, static_cast<float>(costmap->getCost(mxd, myd)));
+            }
+          }
+        } else {
+          cost = static_cast<float>(costmap->getCost(new_idx));
+        }
 
         if (!is_circular) {
           // Adjust cost value if using SE2 footprint checks

diff --git a/nav2_smac_planner/test/test_nodehybrid.cpp b/nav2_smac_planner/test/test_nodehybrid.cpp
@@ -50,7 +50,7 @@ TEST(NodeHybridTest, test_node_hybrid)
 
   // Check defaulted constants
   nav2_smac_planner::NodeHybrid testA(49);
-  EXPECT_EQ(testA.travel_distance_cost, sqrt(2));
+  EXPECT_EQ(testA.travel_distance_cost, sqrtf(2));
 
   nav2_smac_planner::NodeHybrid::initMotionModel(
     nav2_smac_planner::MotionModel::DUBIN, size_x, size_y, size_theta, info);