State Lattice Planner Work Revival

nav2_navfn_planner/src/navfn_planner.cpp

@@ -39,6 +39,7 @@
 #include "nav2_costmap_2d/cost_values.hpp"
 
 using namespace std::chrono_literals;
+using namespace std::chrono;  // NOLINT
 using nav2_util::declare_parameter_if_not_declared;
 using rcl_interfaces::msg::ParameterType;
 using std::placeholders::_1;
@@ -180,8 +181,7 @@ nav_msgs::msg::Path NavfnPlanner::createPlan(
 #ifdef BENCHMARK_TESTING
   steady_clock::time_point b = steady_clock::now();
   duration<double> time_span = duration_cast<duration<double>>(b - a);
-  std::cout << "It took " << time_span.count() * 1000 <<
-    " milliseconds with " << num_iterations << " iterations." << std::endl;
+  std::cout << "It took " << time_span.count() * 1000 << std::endl;
 #endif
 
   return path;

nav2_smac_planner/CMakeLists.txt

@@ -21,6 +21,7 @@ find_package(nav2_costmap_2d REQUIRED)
 find_package(pluginlib REQUIRED)
 find_package(eigen3_cmake_module REQUIRED)
 find_package(Eigen3 REQUIRED)
+find_package(angles REQUIRED)
 find_package(ompl REQUIRED)
 find_package(OpenMP REQUIRED)
 
@@ -66,16 +67,22 @@ set(dependencies
   nav2_costmap_2d
   nav2_core
   pluginlib
+  angles
   eigen3_cmake_module
 )
 
 # Hybrid plugin
 add_library(${library_name} SHARED
   src/smac_planner_hybrid.cpp
   src/a_star.cpp
+  src/collision_checker.cpp
+  src/smoother.cpp
+  src/analytic_expansion.cpp
   src/node_hybrid.cpp
+  src/node_lattice.cpp
   src/costmap_downsampler.cpp
   src/node_2d.cpp
+  src/node_basic.cpp
 )
 
 target_link_libraries(${library_name} ${OMPL_LIBRARIES} ${OpenMP_LIBRARIES}  OpenMP::OpenMP_CXX)
@@ -89,9 +96,14 @@ ament_target_dependencies(${library_name}
 add_library(${library_name}_2d SHARED
   src/smac_planner_2d.cpp
   src/a_star.cpp
+  src/smoother.cpp
+  src/collision_checker.cpp
+  src/analytic_expansion.cpp
   src/node_hybrid.cpp
+  src/node_lattice.cpp
   src/costmap_downsampler.cpp
   src/node_2d.cpp
+  src/node_basic.cpp
 )
 
 target_link_libraries(${library_name}_2d ${OMPL_LIBRARIES})
@@ -101,10 +113,32 @@ ament_target_dependencies(${library_name}_2d
   ${dependencies}
 )
 
-pluginlib_export_plugin_description_file(nav2_core smac_plugin.xml)
+# Lattice plugin
+add_library(${library_name}_lattice SHARED
+  src/smac_planner_lattice.cpp
+  src/a_star.cpp
+  src/smoother.cpp
+  src/collision_checker.cpp
+  src/analytic_expansion.cpp
+  src/node_hybrid.cpp
+  src/node_lattice.cpp
+  src/costmap_downsampler.cpp
+  src/node_2d.cpp
+  src/node_basic.cpp
+)
+
+target_link_libraries(${library_name}_lattice ${OMPL_LIBRARIES})
+target_include_directories(${library_name}_lattice PUBLIC ${Eigen3_INCLUDE_DIRS})
+
+ament_target_dependencies(${library_name}_lattice
+  ${dependencies}
+)
+
+pluginlib_export_plugin_description_file(nav2_core smac_plugin_hybrid.xml)
 pluginlib_export_plugin_description_file(nav2_core smac_plugin_2d.xml)
+pluginlib_export_plugin_description_file(nav2_core smac_plugin_lattice.xml)
 
-install(TARGETS ${library_name} ${library_name}_2d
+install(TARGETS ${library_name} ${library_name}_2d ${library_name}_lattice
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION lib/${PROJECT_NAME}
@@ -122,6 +156,6 @@ if(BUILD_TESTING)
 endif()
 
 ament_export_include_directories(include)
-ament_export_libraries(${library_name} ${library_name}_2d)
+ament_export_libraries(${library_name} ${library_name}_2d ${library_name}_lattice)
 ament_export_dependencies(${dependencies})
 ament_package()

nav2_smac_planner/include/nav2_smac_planner/a_star.hpp

@@ -26,8 +26,10 @@
 
 #include "nav2_costmap_2d/costmap_2d.hpp"
 
+#include "nav2_smac_planner/analytic_expansion.hpp"
 #include "nav2_smac_planner/node_2d.hpp"
 #include "nav2_smac_planner/node_hybrid.hpp"
+#include "nav2_smac_planner/node_lattice.hpp"
 #include "nav2_smac_planner/node_basic.hpp"
 #include "nav2_smac_planner/types.hpp"
 #include "nav2_smac_planner/constants.hpp"
@@ -52,27 +54,6 @@ class AStarAlgorithm
   typedef typename NodeVector::iterator NeighborIterator;
   typedef std::function<bool (const unsigned int &, NodeT * &)> NodeGetter;
 
-  /**
-   * @struct nav2_smac_planner::AnalyticExpansionNodes
-   * @brief Analytic expansion nodes and associated metadata
-   */
-
-  struct AnalyticExpansionNode
-  {
-    AnalyticExpansionNode(
-      NodePtr & node_in,
-      Coordinates & initial_coords_in,
-      Coordinates & proposed_coords_in)
-    : node(node_in), initial_coords(initial_coords_in), proposed_coords(proposed_coords_in)
-    {}
-
-    NodePtr node;
-    Coordinates initial_coords;
-    Coordinates proposed_coords;
-  };
-
-  typedef std::vector<AnalyticExpansionNode> AnalyticExpansionNodes;
-
   /**
    * @struct nav2_smac_planner::NodeComparator
    * @brief Node comparison for priority queue sorting
@@ -229,14 +210,6 @@ class AStarAlgorithm
    */
   inline bool isGoal(NodePtr & node);
 
-  /**
-   * @brief Set the starting pose for planning, as a node index
-   * @param node Node pointer to the goal node to backtrace
-   * @param path Reference to a vector of indicies of generated path
-   * @return whether the path was able to be backtraced
-   */
-  bool backtracePath(NodePtr node, CoordinateVector & path);
-
   /**
    * @brief Get cost of heuristic of node
    * @param node Node index current
@@ -261,31 +234,6 @@ class AStarAlgorithm
    */
   inline void clearGraph();
 
-  /**
-   * @brief Attempt an analytic path completion
-   * @return Node pointer reference to goal node if successful, else
-   * return nullptr
-   */
-  NodePtr tryAnalyticExpansion(
-    const NodePtr & current_node,
-    const NodeGetter & getter, int & iterations, int & best_cost);
-
-  /**
-   * @brief Perform an analytic path expansion to the goal
-   * @param node The node to start the analytic path from
-   * @param getter The function object that gets valid nodes from the graph
-   * @return A set of analytically expanded nodes to the goal from current node, if possible
-   */
-  AnalyticExpansionNodes getAnalyticPath(const NodePtr & node, const NodeGetter & getter);
-
-  /**
-   * @brief Takes final analytic expansion and appends to current expanded node
-   * @param node The node to start the analytic path from
-   * @param expanded_nodes Expanded nodes to append to end of current search path
-   * @return Node pointer to goal node if successful, else return nullptr
-   */
-  NodePtr setAnalyticPath(const NodePtr & node, const AnalyticExpansionNodes & expanded_nodes);
-
   int _timing_interval = 5000;
 
   bool _traverse_unknown;
@@ -310,6 +258,7 @@ class AStarAlgorithm
 
   GridCollisionChecker * _collision_checker;
   nav2_costmap_2d::Costmap2D * _costmap;
+  std::unique_ptr<AnalyticExpansion<NodeT>> _expander;
 };
 
 }  // namespace nav2_smac_planner

nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp

@@ -0,0 +1,130 @@
+// Copyright (c) 2021, Samsung Research America
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License. Reserved.
+
+#ifndef NAV2_SMAC_PLANNER__ANALYTIC_EXPANSION_HPP_
+#define NAV2_SMAC_PLANNER__ANALYTIC_EXPANSION_HPP_
+
+#include <string>
+#include <vector>
+
+#include "nav2_smac_planner/node_2d.hpp"
+#include "nav2_smac_planner/node_hybrid.hpp"
+#include "nav2_smac_planner/node_lattice.hpp"
+#include "nav2_smac_planner/types.hpp"
+#include "nav2_smac_planner/constants.hpp"
+
+namespace nav2_smac_planner
+{
+
+template<typename NodeT>
+class AnalyticExpansion
+{
+public:
+  typedef NodeT * NodePtr;
+  typedef typename NodeT::Coordinates Coordinates;
+  typedef std::function<bool (const unsigned int &, NodeT * &)> NodeGetter;
+
+  /**
+   * @struct nav2_smac_planner::AnalyticExpansion::AnalyticExpansionNodes
+   * @brief Analytic expansion nodes and associated metadata
+   */
+  struct AnalyticExpansionNode
+  {
+    AnalyticExpansionNode(
+      NodePtr & node_in,
+      Coordinates & initial_coords_in,
+      Coordinates & proposed_coords_in)
+    : node(node_in),
+      initial_coords(initial_coords_in),
+      proposed_coords(proposed_coords_in)
+    {
+    }
+
+    NodePtr node;
+    Coordinates initial_coords;
+    Coordinates proposed_coords;
+  };
+
+  typedef std::vector<AnalyticExpansionNode> AnalyticExpansionNodes;
+
+  /**
+   * @brief Constructor for analytic expansion object
+   */
+  AnalyticExpansion(
+    const MotionModel & motion_model,
+    const SearchInfo & search_info,
+    const bool & traverse_unknown,
+    const unsigned int & dim_3_size);
+
+  /**
+   * @brief Sets the collision checker and costmap to use in expansion validation
+   * @param collision_checker Collision checker to use
+   */
+  void setCollisionChecker(GridCollisionChecker * collision_checker);
+
+  /**
+   * @brief Attempt an analytic path completion
+   * @param node The node to start the analytic path from
+   * @param goal The goal node to plan to
+   * @param getter Gets a node at a set of coordinates
+   * @param iterations Iterations to run over
+   * @param best_cost Best heuristic cost to propertionally expand more closer to the goal
+   * @return Node pointer reference to goal node if successful, else
+   * return nullptr
+   */
+  NodePtr tryAnalyticExpansion(
+    const NodePtr & current_node,
+    const NodePtr & goal_node,
+    const NodeGetter & getter, int & iterations, int & best_cost);
+
+  /**
+   * @brief Perform an analytic path expansion to the goal
+   * @param node The node to start the analytic path from
+   * @param goal The goal node to plan to
+   * @param getter The function object that gets valid nodes from the graph
+   * @return A set of analytically expanded nodes to the goal from current node, if possible
+   */
+  AnalyticExpansionNodes getAnalyticPath(
+    const NodePtr & node, const NodePtr & goal,
+    const NodeGetter & getter);
+
+  /**
+   * @brief Takes final analytic expansion and appends to current expanded node
+   * @param node The node to start the analytic path from
+   * @param goal The goal node to plan to
+   * @param expanded_nodes Expanded nodes to append to end of current search path
+   * @return Node pointer to goal node if successful, else return nullptr
+   */
+  NodePtr setAnalyticPath(
+    const NodePtr & node, const NodePtr & goal,
+    const AnalyticExpansionNodes & expanded_nodes);
+
+  /**
+   * @brief Takes an expanded nodes to clean up, if necessary, of any state
+   * information that may be poluting it from a prior search iteration
+   * @param expanded_nodes Expanded node to clean up from search
+   */
+  void cleanNode(const NodePtr & nodes);
+
+protected:
+  MotionModel _motion_model;
+  SearchInfo _search_info;
+  bool _traverse_unknown;
+  unsigned int _dim_3_size;
+  GridCollisionChecker * _collision_checker;
+};
+
+}  // namespace nav2_smac_planner
+
+#endif  // NAV2_SMAC_PLANNER__ANALYTIC_EXPANSION_HPP_