From c09708845e2327dfee1dac7d0a874fd9bf7f2598 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Thu, 11 Apr 2024 15:45:47 -0700 Subject: [PATCH] Smac Planner Optimizations; 9.1% improvement in speed (#4257) * optimizations * Update test_nodehybrid.cpp Signed-off-by: Steve Macenski --------- Signed-off-by: Steve Macenski --- nav2_smac_planner/CMakeLists.txt | 13 +--- .../include/nav2_smac_planner/node_hybrid.hpp | 4 +- nav2_smac_planner/src/a_star.cpp | 2 +- nav2_smac_planner/src/analytic_expansion.cpp | 2 +- nav2_smac_planner/src/collision_checker.cpp | 6 +- nav2_smac_planner/src/node_hybrid.cpp | 71 +++++++++++++------ nav2_smac_planner/test/test_nodehybrid.cpp | 2 +- 7 files changed, 59 insertions(+), 41 deletions(-) diff --git a/nav2_smac_planner/CMakeLists.txt b/nav2_smac_planner/CMakeLists.txt index 4bbe5c55ca..d9da9fd147 100644 --- 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 index c46a947186..b6a33a444a 100644 --- 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 costmap_ros; static std::shared_ptr 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 index 1f0d720763..0470bfd302 100644 --- a/nav2_smac_planner/src/a_star.cpp +++ b/nav2_smac_planner/src/a_star.cpp @@ -365,7 +365,7 @@ bool AStarAlgorithm::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 index e14033d0f1..5f5f97d9ec 100644 --- a/nav2_smac_planner/src/analytic_expansion.cpp +++ b/nav2_smac_planner/src/analytic_expansion.cpp @@ -180,7 +180,7 @@ typename AnalyticExpansion::AnalyticExpansionNodes AnalyticExpansiondistance(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 index c9ce030fab..7e0396bddc 100644 --- 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(costmap_->getCost( - static_cast(x + 0.5), static_cast(y + 0.5))); + static_cast(x + 0.5f), static_cast(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(costmap_->getCost( - static_cast(x + 0.5), static_cast(y + 0.5))); + static_cast(x + 0.5f), static_cast(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 index aa1773e682..63f121de3d 100644 --- 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 NodeHybrid::costmap_ros = nullptr; std::shared_ptr 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 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(costmap->getSizeInCellsX()) / 2.0f); + size = size_x * + ceil(static_cast(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(costmap->getSizeInCellsX()) / 2.0f); + size_y = ceil(static_cast(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(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(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(costmap->getCost(mxd, myd))); + } + } + } else { + cost = static_cast(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 index 1c127c4799..f6d917c08f 100644 --- 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);