Hybrid-A* optimizations and completion without on-going State Lattice work

nav2_costmap_2d/include/nav2_costmap_2d/footprint_collision_checker.hpp

@@ -72,6 +72,13 @@ class FootprintCollisionChecker
   * @brief Set the current costmap object to use for collision detection
   */
   void setCostmap(CostmapT costmap);
+  /**
+  * @brief Get the current costmap object
+  */
+  CostmapT getCostmap()
+  {
+    return costmap_;
+  }
 
 protected:
   CostmapT costmap_;

nav2_costmap_2d/src/footprint_collision_checker.cpp

@@ -51,36 +51,40 @@ double FootprintCollisionChecker<CostmapT>::footprintCost(const Footprint footpr
   unsigned int x0, x1, y0, y1;
   double footprint_cost = 0.0;
 
+  // get the cell coord of the first point
+  if (!worldToMap(footprint[0].x, footprint[0].y, x0, y0)) {
+    return static_cast<double>(LETHAL_OBSTACLE);
+  }
+
+  // cache the start to eliminate a worldToMap call
+  unsigned int xstart = x0;
+  unsigned int ystart = y0;
+
   // we need to rasterize each line in the footprint
   for (unsigned int i = 0; i < footprint.size() - 1; ++i) {
-    // get the cell coord of the first point
-    if (!worldToMap(footprint[i].x, footprint[i].y, x0, y0)) {
-      return static_cast<double>(LETHAL_OBSTACLE);
-    }
 
     // get the cell coord of the second point
     if (!worldToMap(footprint[i + 1].x, footprint[i + 1].y, x1, y1)) {
       return static_cast<double>(LETHAL_OBSTACLE);
     }
 
     footprint_cost = std::max(lineCost(x0, x1, y0, y1), footprint_cost);
-  }
 
-  // we also need to connect the first point in the footprint to the last point
-  // get the cell coord of the last point
-  if (!worldToMap(footprint.back().x, footprint.back().y, x0, y0)) {
-    return static_cast<double>(LETHAL_OBSTACLE);
-  }
+    // the second point is next iteration's first point
+    x0 = x1;
+    y0 = y1;
 
-  // get the cell coord of the first point
-  if (!worldToMap(footprint.front().x, footprint.front().y, x1, y1)) {
-    return static_cast<double>(LETHAL_OBSTACLE);
+    // if in collision, no need to continue
+    if (footprint_cost == static_cast<double>(INSCRIBED_INFLATED_OBSTACLE) ||
+      footprint_cost == static_cast<double>(LETHAL_OBSTACLE))
+    {
+      return footprint_cost;
+    }
   }
 
-  footprint_cost = std::max(lineCost(x0, x1, y0, y1), footprint_cost);
-
-  // if all line costs are legal... then we can return that the footprint is legal
-  return footprint_cost;
+  // we also need to connect the first point in the footprint to the last point
+  // the last iteration's x1, y1 are the last footprint point's coordinates
+  return std::max(lineCost(xstart, x1, ystart, y1), footprint_cost);
 }
 
 template<typename CostmapT>
@@ -95,6 +99,13 @@ double FootprintCollisionChecker<CostmapT>::lineCost(int x0, int x1, int y0, int
     if (line_cost < point_cost) {
       line_cost = point_cost;
     }
+
+    // if in collision, no need to continue
+    if (line_cost == static_cast<double>(INSCRIBED_INFLATED_OBSTACLE) ||
+      line_cost == static_cast<double>(LETHAL_OBSTACLE))
+    {
+      return line_cost;
+    }
   }
 
   return line_cost;

nav2_smac_planner/CMakeLists.txt

@@ -19,7 +19,6 @@ find_package(builtin_interfaces REQUIRED)
 find_package(tf2_ros REQUIRED)
 find_package(nav2_costmap_2d REQUIRED)
 find_package(pluginlib REQUIRED)
-find_package(Ceres REQUIRED COMPONENTS SuiteSparse)
 find_package(eigen3_cmake_module REQUIRED)
 find_package(Eigen3 REQUIRED)
 find_package(ompl REQUIRED)
@@ -35,12 +34,16 @@ add_compile_options(-O3 -Wextra -Wdeprecated -fPIC)
 
 include_directories(
   include
-  ${CERES_INCLUDES}
   ${OMPL_INCLUDE_DIRS}
   ${OpenMP_INCLUDE_DIRS}
 )
-set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
-set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}")
+
+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)
 
@@ -62,16 +65,16 @@ set(dependencies
   eigen3_cmake_module
 )
 
-# SE2 plugin
+# Hybrid plugin
 add_library(${library_name} SHARED
-  src/smac_planner.cpp
+  src/smac_planner_hybrid.cpp
   src/a_star.cpp
-  src/node_se2.cpp
+  src/node_hybrid.cpp
   src/costmap_downsampler.cpp
   src/node_2d.cpp
 )
 
-target_link_libraries(${library_name} ${CERES_LIBRARIES} ${OMPL_LIBRARIES} ${OpenMP_LIBRARIES}  OpenMP::OpenMP_CXX)
+target_link_libraries(${library_name} ${OMPL_LIBRARIES} ${OpenMP_LIBRARIES}  OpenMP::OpenMP_CXX)
 target_include_directories(${library_name} PUBLIC ${Eigen3_INCLUDE_DIRS})
 
 ament_target_dependencies(${library_name}
@@ -82,12 +85,12 @@ ament_target_dependencies(${library_name}
 add_library(${library_name}_2d SHARED
   src/smac_planner_2d.cpp
   src/a_star.cpp
-  src/node_se2.cpp
+  src/node_hybrid.cpp
   src/costmap_downsampler.cpp
   src/node_2d.cpp
 )
 
-target_link_libraries(${library_name}_2d ${CERES_LIBRARIES} ${OMPL_LIBRARIES})
+target_link_libraries(${library_name}_2d ${OMPL_LIBRARIES})
 target_include_directories(${library_name}_2d PUBLIC ${Eigen3_INCLUDE_DIRS})
 
 ament_target_dependencies(${library_name}_2d