Add support for switching between SMAC planners

nav2_smac_planner/CMakeLists.txt

@@ -42,19 +42,13 @@ add_library(${library_name}_common SHARED
   src/node_basic.cpp
   src/node_hybrid.cpp
   src/node_lattice.cpp
+  src/obstacle_heuristic.cpp
+  src/distance_heuristic.cpp
   src/smoother.cpp
 )
-# Add GenerateExportHeader support for symbol visibility, as we are using
-# static members we need to explicitly export them on Windows, as
-# CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS does not work with static members.
-include(GenerateExportHeader)
-generate_export_header(${library_name}_common
-  EXPORT_FILE_NAME "${CMAKE_CURRENT_BINARY_DIR}/include/${PROJECT_NAME}/${library_name}_common_visibility_control.hpp"
-)
 target_include_directories(${library_name}_common
   PUBLIC
     "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
-    "$<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}/include>"
     "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
     ${OMPL_INCLUDE_DIRS}
 )
@@ -181,11 +175,6 @@ install(TARGETS ${library_name}_common ${library_name} ${library_name}_2d ${libr
 install(DIRECTORY include/
   DESTINATION include/${PROJECT_NAME}
 )
-install(FILES
-  "${CMAKE_CURRENT_BINARY_DIR}/include/${PROJECT_NAME}/${library_name}_common_visibility_control.hpp"
-  DESTINATION include/${PROJECT_NAME}
-)
-
 install(DIRECTORY lattice_primitives/sample_primitives DESTINATION share/${PROJECT_NAME})
 
 if(BUILD_TESTING)

nav2_smac_planner/include/nav2_smac_planner/a_star.hpp

@@ -56,6 +56,7 @@ class AStarAlgorithm
   typedef typename NodeVector::iterator NeighborIterator;
   typedef std::function<bool (const uint64_t &, NodeT * &)> NodeGetter;
   typedef GoalManager<NodeT> GoalManagerT;
+  using NodeContext = typename NodeT::NodeContext;
 
 
   /**
@@ -205,6 +206,12 @@ class AStarAlgorithm
    */
   GoalManagerT getGoalManager();
 
+  /**
+   * @brief Get pointer to shared node context
+   * @return Node context pointer
+   */
+  NodeContext * getContext();
+
 protected:
   /**
    * @brief Get pointer to next goal in open set
@@ -255,6 +262,16 @@ class AStarAlgorithm
    */
   inline void clearGraph();
 
+  /**
+   * @brief Get index at coordinates
+   * @param x X coordinate of point
+   * @param y Y coordinate of point
+   * @param dim3 Z coordinate / theta bin of point
+   * @return Index
+   */
+  inline uint64_t getIndex(
+    const unsigned int & x, const unsigned int & y, const unsigned int & dim3);
+
   /**
    * @brief Check if node has been visited
    * @param current_node Node to check if visited
@@ -294,6 +311,7 @@ class AStarAlgorithm
   GridCollisionChecker * _collision_checker;
   nav2_costmap_2d::Costmap2D * _costmap;
   std::unique_ptr<AnalyticExpansion<NodeT>> _expander;
+  std::shared_ptr<NodeContext> _shared_ctx;
 };
 
 }  // namespace nav2_smac_planner

nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp

@@ -43,6 +43,7 @@ class AnalyticExpansion
   typedef typename NodeT::Coordinates Coordinates;
   typedef std::function<bool (const uint64_t &, NodeT * &)> NodeGetter;
   typedef typename NodeT::CoordinateVector CoordinateVector;
+  using NodeContext = typename NodeT::NodeContext;
 
   /**
    * @struct nav2_smac_planner::AnalyticExpansion::AnalyticExpansionNodes
@@ -108,6 +109,12 @@ class AnalyticExpansion
    */
   void setCollisionChecker(GridCollisionChecker * collision_checker);
 
+  /**
+   * @brief Sets the shared context to use
+   * @param ctx Shared context pointer
+   */
+  void setContext(NodeContext * ctx);
+
   /**
    * @brief Attempt an analytic path completion
    * @param node The node to start the analytic path from
@@ -187,6 +194,7 @@ class AnalyticExpansion
   unsigned int _dim_3_size;
   GridCollisionChecker * _collision_checker;
   std::list<std::unique_ptr<NodeT>> _detached_nodes;
+  NodeContext * _ctx = nullptr;
 };
 
 }  // namespace nav2_smac_planner

nav2_smac_planner/include/nav2_smac_planner/distance_heuristic.hpp

@@ -0,0 +1,79 @@
+// Copyright (c) 2026, Open Navigation LLC
+//
+// 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__DISTANCE_HEURISTIC_HPP_
+#define NAV2_SMAC_PLANNER__DISTANCE_HEURISTIC_HPP_
+
+#include "nav2_smac_planner/constants.hpp"
+#include "nav2_smac_planner/types.hpp"
+
+namespace nav2_smac_planner
+{
+struct HybridMotionTable;
+struct LatticeMotionTable;
+class NodeHybrid;
+class NodeLattice;
+
+/**
+ * @class nav2_smac_planner::DistanceHeuristic
+ * @brief Distance Heuristic implementation for graph, Hybrid-A*
+ */
+template<typename NodeT>
+class DistanceHeuristic
+{
+public:
+  /**
+   * @brief A constructor for nav2_smac_planner::DistanceHeuristic
+   */
+  DistanceHeuristic() {}
+
+  /**
+   * @brief Compute the SE2 distance heuristic
+   * @param lookup_table_dim Size, in costmap pixels, of the
+   * each lookup table dimension to populate
+   * @param motion_model Motion model to use for state space
+   * @param dim_3_size Number of quantization bins for caching
+   * @param search_info Info containing minimum radius to use
+   */
+  template<typename MotionTableT>
+  void precomputeDistanceHeuristic(
+    const float & lookup_table_dim,
+    const MotionModel & motion_model,
+    const unsigned int & dim_3_size,
+    const SearchInfo & search_info,
+    MotionTableT & motion_table);
+
+  /**
+   * @brief Compute the Distance heuristic
+   * @param node_coords Coordinates to get heuristic at
+   * @param goal_coords Coordinates to compute heuristic to
+   * @param obstacle_heuristic Value of the obstacle heuristic to compute
+   * additional motion heuristics if required
+   * @return heuristic Heuristic value
+   */
+  template<typename MotionTableT>
+  float getDistanceHeuristic(
+    const Coordinates & node_coords,
+    const Coordinates & goal_coords,
+    const float & obstacle_heuristic,
+    MotionTableT & motion_table);
+
+protected:
+  // Dubin / Reeds-Shepp lookup and size for dereferencing
+  LookupTable dist_heuristic_lookup_table_;
+  float size_lookup_;
+};
+
+}  // namespace nav2_smac_planner
+#endif  // NAV2_SMAC_PLANNER__DISTANCE_HEURISTIC_HPP_

nav2_smac_planner/include/nav2_smac_planner/goal_manager.hpp

@@ -47,6 +47,7 @@ class GoalManager
   typedef std::vector<GoalState<NodeT>> GoalStateVector;
   typedef typename NodeT::Coordinates Coordinates;
   typedef typename NodeT::CoordinateVector CoordinateVector;
+  using NodeContext = typename NodeT::NodeContext;
 
   /**
   * @brief Constructor: Initializes empty goal state. sets and coordinate lists.
@@ -64,6 +65,15 @@ class GoalManager
    */
   ~GoalManager() = default;
 
+  /**
+   * @brief Sets the node context for goal nodes
+   * @param ctx Pointer to the NodeContext
+   */
+  void setContext(NodeContext * ctx)
+  {
+    _ctx = ctx;
+  }
+
   /**
    * @brief Checks if the goals set is empty
    * @return true if the goals set is empty
@@ -134,15 +144,16 @@ class GoalManager
     const auto size_x = collision_checker->getCostmap()->getSizeInCellsX();
     const auto size_y = collision_checker->getCostmap()->getSizeInCellsY();
 
-    auto getIndexFromPoint = [&size_x](const Coordinates & point) {
+    auto getIndexFromPoint = [this, &size_x](const Coordinates & point) {
         unsigned int index = 0;
 
         const auto mx = static_cast<unsigned int>(point.x);
         const auto my = static_cast<unsigned int>(point.y);
 
         if constexpr (!std::is_same_v<NodeT, Node2D>) {
           const auto angle = static_cast<unsigned int>(point.theta);
-          index = NodeT::getIndex(mx, my, angle);
+          index = NodeT::getIndex(mx, my, angle, _ctx->motion_table.size_x,
+                                  _ctx->motion_table.num_angle_quantization);
         } else {
           index = NodeT::getIndex(mx, my, size_x);
         }
@@ -169,7 +180,7 @@ class GoalManager
           continue;
         }
 
-        NodeT current_node(getIndexFromPoint(m));
+        NodeT current_node(getIndexFromPoint(m), _ctx);
         current_node.setPose(m);
 
         if (current_node.isNodeValid(traverse_unknown, collision_checker)) {
@@ -281,6 +292,7 @@ class GoalManager
   GoalStateVector _goals_state;
   CoordinateVector _goals_coordinate;
   Coordinates _ref_goal_coord;
+  NodeContext * _ctx = nullptr;
 };
 
 }  // namespace nav2_smac_planner

nav2_smac_planner/include/nav2_smac_planner/node_2d.hpp

@@ -38,37 +38,20 @@ class Node2D
   typedef Node2D * NodePtr;
   typedef std::unique_ptr<std::vector<Node2D>> Graph;
   typedef std::vector<NodePtr> NodeVector;
+  using Coordinates = nav2_smac_planner::Coordinates2D;
+  typedef std::vector<Coordinates> CoordinateVector;
 
-  /**
-   * @class nav2_smac_planner::Node2D::Coordinates
-   * @brief Node2D implementation of coordinate structure
-   */
-  struct Coordinates
+  struct NodeContext
   {
-    Coordinates() {}
-    Coordinates(const float & x_in, const float & y_in)
-    : x(x_in), y(y_in)
-    {}
-
-    inline bool operator==(const Coordinates & rhs) const
-    {
-      return this->x == rhs.x && this->y == rhs.y;
-    }
-
-    inline bool operator!=(const Coordinates & rhs) const
-    {
-      return !(*this == rhs);
-    }
-
-    float x, y;
+    float cost_travel_multiplier;
+    std::vector<int> neighbors_grid_offsets;
   };
-  typedef std::vector<Coordinates> CoordinateVector;
 
   /**
    * @brief A constructor for nav2_smac_planner::Node2D
    * @param index The index of this node for self-reference
    */
-  explicit Node2D(const uint64_t index);
+  explicit Node2D(const uint64_t index, NodeContext * ctx);
 
   /**
    * @brief A destructor for nav2_smac_planner::Node2D
@@ -229,9 +212,9 @@ class Node2D
    * @param Index Index of point
    * @return coordinates of point
    */
-  static inline Coordinates getCoords(const uint64_t & index)
+  inline Coordinates getCoords(const uint64_t & index)
   {
-    const unsigned int & size_x = _neighbors_grid_offsets[3];
+    const unsigned int & size_x = _ctx->neighbors_grid_offsets[3];
     return Coordinates(index % size_x, index / size_x);
   }
 
@@ -241,7 +224,7 @@ class Node2D
    * @param node Node index of new
    * @return Heuristic cost between the nodes
    */
-  static float getHeuristicCost(
+  float getHeuristicCost(
     const Coordinates & node_coords,
     const CoordinateVector & goals_coords);
 
@@ -255,6 +238,7 @@ class Node2D
    * @param search_info Search parameters, unused by 2D node
    */
   static void initMotionModel(
+    NodeContext * ctx,
     const MotionModel & motion_model,
     unsigned int & size_x,
     unsigned int & size_y,
@@ -284,8 +268,6 @@ class Node2D
 
   Node2D * parent;
   Coordinates pose;
-  static float cost_travel_multiplier;
-  static std::vector<int> _neighbors_grid_offsets;
 
 private:
   float _cell_cost;
@@ -294,6 +276,7 @@ class Node2D
   bool _was_visited;
   bool _is_queued;
   bool _in_collision{false};
+  NodeContext * _ctx = nullptr;
 };
 
 }  // namespace nav2_smac_planner