From e6bc3cdafde9d3edb80bd3cfe44d73fc92e9a914 Mon Sep 17 00:00:00 2001
From: Tony Najjar <tony.najjar.1997@gmail.com>
Date: Tue, 17 Nov 2020 00:26:14 +0100
Subject: [PATCH] Added nav2_ prefix to smac_planner Signed-off-by: Tony Najjar
 <tony.najjar.1997@gmail.com> (#2093)

---
 .../CMakeLists.txt                            |   4 +-
 {smac_planner => nav2_smac_planner}/README.md |   4 +-
 .../include/nav2_smac_planner}/a_star.hpp     |  28 +--
 .../nav2_smac_planner}/collision_checker.hpp  |  16 +-
 .../include/nav2_smac_planner}/constants.hpp  |  10 +-
 .../costmap_downsampler.hpp                   |  14 +-
 .../include/nav2_smac_planner}/node_2d.hpp    |  24 +-
 .../include/nav2_smac_planner}/node_basic.hpp |  24 +-
 .../include/nav2_smac_planner}/node_se2.hpp   |  40 ++--
 .../include/nav2_smac_planner}/options.hpp    |  16 +-
 .../nav2_smac_planner}/smac_planner.hpp       |  16 +-
 .../nav2_smac_planner}/smac_planner_2d.hpp    |  16 +-
 .../include/nav2_smac_planner}/smoother.hpp   |  20 +-
 .../smoother_cost_function.hpp                |  20 +-
 .../include/nav2_smac_planner}/types.hpp      |  12 +-
 .../package.xml                               |   2 +-
 nav2_smac_planner/smac_plugin.xml             |   5 +
 nav2_smac_planner/smac_plugin_2d.xml          |   5 +
 .../src/a_star.cpp                            |   6 +-
 .../src/costmap_downsampler.cpp               |   6 +-
 .../src/node_2d.cpp                           |   8 +-
 .../src/node_se2.cpp                          |   8 +-
 .../src/smac_planner.cpp                      |   8 +-
 .../src/smac_planner_2d.cpp                   |   8 +-
 .../test/CMakeLists.txt                       |   0
 .../test/deprecated_upsampler/upsampler.hpp   |  16 +-
 .../upsampler_cost_function.hpp               |  14 +-
 .../upsampler_cost_function_nlls.hpp          |  14 +-
 .../test/path.png                             | Bin
 .../test/test_a_star.cpp                      |  61 ++---
 .../test/test_collision_checker.cpp           |  12 +-
 .../test/test_costmap_downsampler.cpp         |   4 +-
 .../test/test_node2d.cpp                      |  72 +++---
 .../test/test_nodebasic.cpp                   |  12 +-
 nav2_smac_planner/test/test_nodese2.cpp       | 214 ++++++++++++++++++
 .../test/test_smac_2d.cpp                     |  12 +-
 .../test/test_smac_se2.cpp                    |  14 +-
 .../test/test_smoother.cpp                    |  18 +-
 navigation2/package.xml                       |   2 +-
 smac_planner/smac_plugin.xml                  |   5 -
 smac_planner/smac_plugin_2d.xml               |   5 -
 smac_planner/test/test_nodese2.cpp            | 214 ------------------
 42 files changed, 507 insertions(+), 502 deletions(-)
 rename {smac_planner => nav2_smac_planner}/CMakeLists.txt (98%)
 rename {smac_planner => nav2_smac_planner}/README.md (93%)
 rename {smac_planner/include/smac_planner => nav2_smac_planner/include/nav2_smac_planner}/a_star.hpp (93%)
 rename {smac_planner/include/smac_planner => nav2_smac_planner/include/nav2_smac_planner}/collision_checker.hpp (89%)
 rename {smac_planner/include/smac_planner => nav2_smac_planner/include/nav2_smac_planner}/constants.hpp (89%)
 rename {smac_planner/include/smac_planner => nav2_smac_planner/include/nav2_smac_planner}/costmap_downsampler.hpp (91%)
 rename {smac_planner/include/smac_planner => nav2_smac_planner/include/nav2_smac_planner}/node_2d.hpp (90%)
 rename {smac_planner/include/smac_planner => nav2_smac_planner/include/nav2_smac_planner}/node_basic.hpp (72%)
 rename {smac_planner/include/smac_planner => nav2_smac_planner/include/nav2_smac_planner}/node_se2.hpp (90%)
 rename {smac_planner/include/smac_planner => nav2_smac_planner/include/nav2_smac_planner}/options.hpp (95%)
 rename {smac_planner/include/smac_planner => nav2_smac_planner/include/nav2_smac_planner}/smac_planner.hpp (91%)
 rename {smac_planner/include/smac_planner => nav2_smac_planner/include/nav2_smac_planner}/smac_planner_2d.hpp (90%)
 rename {smac_planner/include/smac_planner => nav2_smac_planner/include/nav2_smac_planner}/smoother.hpp (90%)
 rename {smac_planner/include/smac_planner => nav2_smac_planner/include/nav2_smac_planner}/smoother_cost_function.hpp (96%)
 rename {smac_planner/include/smac_planner => nav2_smac_planner/include/nav2_smac_planner}/types.hpp (81%)
 rename {smac_planner => nav2_smac_planner}/package.xml (97%)
 create mode 100644 nav2_smac_planner/smac_plugin.xml
 create mode 100644 nav2_smac_planner/smac_plugin_2d.xml
 rename {smac_planner => nav2_smac_planner}/src/a_star.cpp (99%)
 rename {smac_planner => nav2_smac_planner}/src/costmap_downsampler.cpp (97%)
 rename {smac_planner => nav2_smac_planner}/src/node_2d.cpp (96%)
 rename {smac_planner => nav2_smac_planner}/src/node_se2.cpp (98%)
 rename {smac_planner => nav2_smac_planner}/src/smac_planner.cpp (98%)
 rename {smac_planner => nav2_smac_planner}/src/smac_planner_2d.cpp (98%)
 rename {smac_planner => nav2_smac_planner}/test/CMakeLists.txt (100%)
 rename {smac_planner => nav2_smac_planner}/test/deprecated_upsampler/upsampler.hpp (94%)
 rename {smac_planner => nav2_smac_planner}/test/deprecated_upsampler/upsampler_cost_function.hpp (97%)
 rename {smac_planner => nav2_smac_planner}/test/deprecated_upsampler/upsampler_cost_function_nlls.hpp (96%)
 rename {smac_planner => nav2_smac_planner}/test/path.png (100%)
 rename {smac_planner => nav2_smac_planner}/test/test_a_star.cpp (73%)
 rename {smac_planner => nav2_smac_planner}/test/test_collision_checker.cpp (92%)
 rename {smac_planner => nav2_smac_planner}/test/test_costmap_downsampler.cpp (95%)
 rename {smac_planner => nav2_smac_planner}/test/test_node2d.cpp (52%)
 rename {smac_planner => nav2_smac_planner}/test/test_nodebasic.cpp (79%)
 create mode 100644 nav2_smac_planner/test/test_nodese2.cpp
 rename {smac_planner => nav2_smac_planner}/test/test_smac_2d.cpp (89%)
 rename {smac_planner => nav2_smac_planner}/test/test_smac_se2.cpp (87%)
 rename {smac_planner => nav2_smac_planner}/test/test_smoother.cpp (87%)
 delete mode 100644 smac_planner/smac_plugin.xml
 delete mode 100644 smac_planner/smac_plugin_2d.xml
 delete mode 100644 smac_planner/test/test_nodese2.cpp

diff --git a/smac_planner/CMakeLists.txt b/nav2_smac_planner/CMakeLists.txt
similarity index 98%
rename from smac_planner/CMakeLists.txt
rename to nav2_smac_planner/CMakeLists.txt
index d161ba51..e15c474d 100644
--- a/smac_planner/CMakeLists.txt
+++ b/nav2_smac_planner/CMakeLists.txt
@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.5)
-project(smac_planner)
+project(nav2_smac_planner)
 
 set(CMAKE_BUILD_TYPE Release) #Debug, Release
 
@@ -42,7 +42,7 @@ include_directories(
 set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
 set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}")
 
-set(library_name smac_planner)
+set(library_name nav2_smac_planner)
 
 set(dependencies
   rclcpp
diff --git a/smac_planner/README.md b/nav2_smac_planner/README.md
similarity index 93%
rename from smac_planner/README.md
rename to nav2_smac_planner/README.md
index 5bf0cd42..42beb895 100644
--- a/smac_planner/README.md
+++ b/nav2_smac_planner/README.md
@@ -16,7 +16,7 @@ We have users reporting using this on:
 
 ## Introduction
 
-The `smac_planner` package contains an optimized templated A* search algorithm used to create multiple A\*-based planners for multiple types of robot platforms. It was built by [Steve Macenski](https://www.linkedin.com/in/steve-macenski-41a985101/) while at [Samsung Research](https://www.sra.samsung.com/). We support differential-drive and omni-directional drive robots using the `SmacPlanner2D` planner which implements a cost-aware A\* planner. We support cars, car-like, and ackermann vehicles using the `SmacPlanner` plugin which implements a Hybrid-A\* planner. This plugin is also useful for curvature constrained planning, like when planning robot at high speeds to make sure they don't flip over or otherwise skid out of control. It is also applicable to non-round robots (such as large rectangular or arbitrary shaped robots of differential/omnidirectional drivetrains) that need pose-based collision checking.
+The `nav2_smac_planner` package contains an optimized templated A* search algorithm used to create multiple A\*-based planners for multiple types of robot platforms. It was built by [Steve Macenski](https://www.linkedin.com/in/steve-macenski-41a985101/) while at [Samsung Research](https://www.sra.samsung.com/). We support differential-drive and omni-directional drive robots using the `SmacPlanner2D` planner which implements a cost-aware A\* planner. We support cars, car-like, and ackermann vehicles using the `SmacPlanner` plugin which implements a Hybrid-A\* planner. This plugin is also useful for curvature constrained planning, like when planning robot at high speeds to make sure they don't flip over or otherwise skid out of control. It is also applicable to non-round robots (such as large rectangular or arbitrary shaped robots of differential/omnidirectional drivetrains) that need pose-based collision checking.
 
 The `SmacPlanner` fully-implements the Hybrid-A* planner as proposed in [Practical Search Techniques in Path Planning for Autonomous Driving](https://ai.stanford.edu/~ddolgov/papers/dolgov_gpp_stair08.pdf), including hybrid searching, CG smoothing, analytic expansions and hueristic functions.
 
@@ -88,7 +88,7 @@ planner_server:
     use_sim_time: True
 
     GridBased:
-      plugin: "smac_planner/SmacPlanner"
+      plugin: "nav2_smac_planner/SmacPlanner"
       tolerance: 0.5                    # tolerance for planning if unable to reach exact pose, in meters, for 2D node
       downsample_costmap: false         # whether or not to downsample the map
       downsampling_factor: 1            # multiplier for the resolution of the costmap layer (e.g. 2 on a 5cm costmap would be 10cm)
diff --git a/smac_planner/include/smac_planner/a_star.hpp b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp
similarity index 93%
rename from smac_planner/include/smac_planner/a_star.hpp
rename to nav2_smac_planner/include/nav2_smac_planner/a_star.hpp
index a53b872a..6158422a 100644
--- a/smac_planner/include/smac_planner/a_star.hpp
+++ b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp
@@ -13,8 +13,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License. Reserved.
 
-#ifndef SMAC_PLANNER__A_STAR_HPP_
-#define SMAC_PLANNER__A_STAR_HPP_
+#ifndef NAV2_SMAC_PLANNER__A_STAR_HPP_
+#define NAV2_SMAC_PLANNER__A_STAR_HPP_
 
 #include <vector>
 #include <iostream>
@@ -26,13 +26,13 @@
 
 #include "nav2_costmap_2d/costmap_2d.hpp"
 
-#include "smac_planner/node_2d.hpp"
-#include "smac_planner/node_se2.hpp"
-#include "smac_planner/node_basic.hpp"
-#include "smac_planner/types.hpp"
-#include "smac_planner/constants.hpp"
+#include "nav2_smac_planner/node_2d.hpp"
+#include "nav2_smac_planner/node_se2.hpp"
+#include "nav2_smac_planner/node_basic.hpp"
+#include "nav2_smac_planner/types.hpp"
+#include "nav2_smac_planner/constants.hpp"
 
-namespace smac_planner
+namespace nav2_smac_planner
 {
 
 inline double squaredDistance(
@@ -45,7 +45,7 @@ inline double squaredDistance(
 }
 
 /**
- * @class smac_planner::AStarAlgorithm
+ * @class nav2_smac_planner::AStarAlgorithm
  * @brief An A* implementation for planning in a costmap. Templated based on the Node type.
  */
 template<typename NodeT>
@@ -62,7 +62,7 @@ public:
   typedef std::function<bool (const unsigned int &, NodeT * &)> NodeGetter;
 
   /**
-   * @struct smac_planner::NodeComparator
+   * @struct nav2_smac_planner::NodeComparator
    * @brief Node comparison for priority queue sorting
    */
   struct NodeComparator
@@ -76,13 +76,13 @@ public:
   typedef std::priority_queue<NodeElement, std::vector<NodeElement>, NodeComparator> NodeQueue;
 
   /**
-   * @brief A constructor for smac_planner::PlannerServer
+   * @brief A constructor for nav2_smac_planner::PlannerServer
    * @param neighborhood The type of neighborhood to use for search (4 or 8 connected)
    */
   explicit AStarAlgorithm(const MotionModel & motion_model, const SearchInfo & search_info);
 
   /**
-   * @brief A destructor for smac_planner::AStarAlgorithm
+   * @brief A destructor for nav2_smac_planner::AStarAlgorithm
    */
   ~AStarAlgorithm();
 
@@ -316,6 +316,6 @@ protected:
   nav2_costmap_2d::Costmap2D * _costmap;
 };
 
-}  // namespace smac_planner
+}  // namespace nav2_smac_planner
 
-#endif  // SMAC_PLANNER__A_STAR_HPP_
+#endif  // NAV2_SMAC_PLANNER__A_STAR_HPP_
diff --git a/smac_planner/include/smac_planner/collision_checker.hpp b/nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp
similarity index 89%
rename from smac_planner/include/smac_planner/collision_checker.hpp
rename to nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp
index 58851490..6b77d13a 100644
--- a/smac_planner/include/smac_planner/collision_checker.hpp
+++ b/nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp
@@ -13,16 +13,16 @@
 // limitations under the License. Reserved.
 
 #include "nav2_costmap_2d/footprint_collision_checker.hpp"
-#include "smac_planner/constants.hpp"
+#include "nav2_smac_planner/constants.hpp"
 
-#ifndef SMAC_PLANNER__COLLISION_CHECKER_HPP_
-#define SMAC_PLANNER__COLLISION_CHECKER_HPP_
+#ifndef NAV2_SMAC_PLANNER__COLLISION_CHECKER_HPP_
+#define NAV2_SMAC_PLANNER__COLLISION_CHECKER_HPP_
 
-namespace smac_planner
+namespace nav2_smac_planner
 {
 
 /**
- * @class smac_planner::GridCollisionChecker
+ * @class nav2_smac_planner::GridCollisionChecker
  * @brief A costmap grid collision checker
  */
 class GridCollisionChecker
@@ -30,7 +30,7 @@ class GridCollisionChecker
 {
 public:
   /**
-   * @brief A constructor for smac_planner::GridCollisionChecker
+   * @brief A constructor for nav2_smac_planner::GridCollisionChecker
    * @param costmap The costmap to collision check against
    */
   GridCollisionChecker(
@@ -108,6 +108,6 @@ protected:
   bool footprint_is_radius_;
 };
 
-}  // namespace smac_planner
+}  // namespace nav2_smac_planner
 
-#endif  // SMAC_PLANNER__COLLISION_CHECKER_HPP_
+#endif  // NAV2_SMAC_PLANNER__COLLISION_CHECKER_HPP_
diff --git a/smac_planner/include/smac_planner/constants.hpp b/nav2_smac_planner/include/nav2_smac_planner/constants.hpp
similarity index 89%
rename from smac_planner/include/smac_planner/constants.hpp
rename to nav2_smac_planner/include/nav2_smac_planner/constants.hpp
index bc2ccfd8..1a67c746 100644
--- a/smac_planner/include/smac_planner/constants.hpp
+++ b/nav2_smac_planner/include/nav2_smac_planner/constants.hpp
@@ -12,12 +12,12 @@
 // See the License for the specific language governing permissions and
 // limitations under the License. Reserved.
 
-#ifndef SMAC_PLANNER__CONSTANTS_HPP_
-#define SMAC_PLANNER__CONSTANTS_HPP_
+#ifndef NAV2_SMAC_PLANNER__CONSTANTS_HPP_
+#define NAV2_SMAC_PLANNER__CONSTANTS_HPP_
 
 #include <string>
 
-namespace smac_planner
+namespace nav2_smac_planner
 {
 enum class MotionModel
 {
@@ -65,6 +65,6 @@ const float INSCRIBED = 253;
 const float MAX_NON_OBSTACLE = 252;
 const float FREE = 0;
 
-}  // namespace smac_planner
+}  // namespace nav2_smac_planner
 
-#endif  // SMAC_PLANNER__CONSTANTS_HPP_
+#endif  // NAV2_SMAC_PLANNER__CONSTANTS_HPP_
diff --git a/smac_planner/include/smac_planner/costmap_downsampler.hpp b/nav2_smac_planner/include/nav2_smac_planner/costmap_downsampler.hpp
similarity index 91%
rename from smac_planner/include/smac_planner/costmap_downsampler.hpp
rename to nav2_smac_planner/include/nav2_smac_planner/costmap_downsampler.hpp
index 9521b15f..95d12a7f 100644
--- a/smac_planner/include/smac_planner/costmap_downsampler.hpp
+++ b/nav2_smac_planner/include/nav2_smac_planner/costmap_downsampler.hpp
@@ -12,21 +12,21 @@
 // See the License for the specific language governing permissions and
 // limitations under the License. Reserved.
 
-#ifndef SMAC_PLANNER__COSTMAP_DOWNSAMPLER_HPP_
-#define SMAC_PLANNER__COSTMAP_DOWNSAMPLER_HPP_
+#ifndef NAV2_SMAC_PLANNER__COSTMAP_DOWNSAMPLER_HPP_
+#define NAV2_SMAC_PLANNER__COSTMAP_DOWNSAMPLER_HPP_
 
 #include <algorithm>
 #include <string>
 #include <memory>
 
 #include "nav2_costmap_2d/costmap_2d_ros.hpp"
-#include "smac_planner/constants.hpp"
+#include "nav2_smac_planner/constants.hpp"
 
-namespace smac_planner
+namespace nav2_smac_planner
 {
 
 /**
- * @class smac_planner::CostmapDownsampler
+ * @class nav2_smac_planner::CostmapDownsampler
  * @brief A costmap downsampler for more efficient path planning
  */
 class CostmapDownsampler
@@ -110,6 +110,6 @@ protected:
   std::unique_ptr<nav2_costmap_2d::Costmap2DPublisher> _downsampled_costmap_pub;
 };
 
-}  // namespace smac_planner
+}  // namespace nav2_smac_planner
 
-#endif  // SMAC_PLANNER__COSTMAP_DOWNSAMPLER_HPP_
+#endif  // NAV2_SMAC_PLANNER__COSTMAP_DOWNSAMPLER_HPP_
diff --git a/smac_planner/include/smac_planner/node_2d.hpp b/nav2_smac_planner/include/nav2_smac_planner/node_2d.hpp
similarity index 90%
rename from smac_planner/include/smac_planner/node_2d.hpp
rename to nav2_smac_planner/include/nav2_smac_planner/node_2d.hpp
index b80a316c..688e6ea5 100644
--- a/smac_planner/include/smac_planner/node_2d.hpp
+++ b/nav2_smac_planner/include/nav2_smac_planner/node_2d.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License. Reserved.
 
-#ifndef SMAC_PLANNER__NODE_2D_HPP_
-#define SMAC_PLANNER__NODE_2D_HPP_
+#ifndef NAV2_SMAC_PLANNER__NODE_2D_HPP_
+#define NAV2_SMAC_PLANNER__NODE_2D_HPP_
 
 #include <math.h>
 #include <vector>
@@ -24,14 +24,14 @@
 #include <utility>
 #include <functional>
 
-#include "smac_planner/constants.hpp"
-#include "smac_planner/collision_checker.hpp"
+#include "nav2_smac_planner/constants.hpp"
+#include "nav2_smac_planner/collision_checker.hpp"
 
-namespace smac_planner
+namespace nav2_smac_planner
 {
 
 /**
- * @class smac_planner::Node2D
+ * @class nav2_smac_planner::Node2D
  * @brief Node2D implementation for graph
  */
 class Node2D
@@ -42,7 +42,7 @@ public:
   typedef std::vector<NodePtr> NodeVector;
 
   /**
-   * @class smac_planner::Node2D::Coordinates
+   * @class nav2_smac_planner::Node2D::Coordinates
    * @brief Node2D implementation of coordinate structure
    */
   struct Coordinates
@@ -57,14 +57,14 @@ public:
   typedef std::vector<Coordinates> CoordinateVector;
 
   /**
-   * @brief A constructor for smac_planner::Node2D
+   * @brief A constructor for nav2_smac_planner::Node2D
    * @param cost_in The costmap cost at this node
    * @param index The index of this node for self-reference
    */
   explicit Node2D(unsigned char & cost_in, const unsigned int index);
 
   /**
-   * @brief A destructor for smac_planner::Node2D
+   * @brief A destructor for nav2_smac_planner::Node2D
    */
   ~Node2D();
 
@@ -226,7 +226,7 @@ public:
    */
   static void getNeighbors(
     NodePtr & node,
-    std::function<bool(const unsigned int &, smac_planner::Node2D * &)> & validity_checker,
+    std::function<bool(const unsigned int &, nav2_smac_planner::Node2D * &)> & validity_checker,
     GridCollisionChecker collision_checker,
     const bool & traverse_unknown,
     NodeVector & neighbors);
@@ -243,6 +243,6 @@ private:
   bool _is_queued;
 };
 
-}  // namespace smac_planner
+}  // namespace nav2_smac_planner
 
-#endif  // SMAC_PLANNER__NODE_2D_HPP_
+#endif  // NAV2_SMAC_PLANNER__NODE_2D_HPP_
diff --git a/smac_planner/include/smac_planner/node_basic.hpp b/nav2_smac_planner/include/nav2_smac_planner/node_basic.hpp
similarity index 72%
rename from smac_planner/include/smac_planner/node_basic.hpp
rename to nav2_smac_planner/include/nav2_smac_planner/node_basic.hpp
index 3f394171..290ca131 100644
--- a/smac_planner/include/smac_planner/node_basic.hpp
+++ b/nav2_smac_planner/include/nav2_smac_planner/node_basic.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License. Reserved.
 
-#ifndef SMAC_PLANNER__NODE_BASIC_HPP_
-#define SMAC_PLANNER__NODE_BASIC_HPP_
+#ifndef NAV2_SMAC_PLANNER__NODE_BASIC_HPP_
+#define NAV2_SMAC_PLANNER__NODE_BASIC_HPP_
 
 #include <math.h>
 #include <vector>
@@ -27,17 +27,17 @@
 
 #include "ompl/base/StateSpace.h"
 
-#include "smac_planner/constants.hpp"
-#include "smac_planner/node_se2.hpp"
-#include "smac_planner/node_2d.hpp"
-#include "smac_planner/types.hpp"
-#include "smac_planner/collision_checker.hpp"
+#include "nav2_smac_planner/constants.hpp"
+#include "nav2_smac_planner/node_se2.hpp"
+#include "nav2_smac_planner/node_2d.hpp"
+#include "nav2_smac_planner/types.hpp"
+#include "nav2_smac_planner/collision_checker.hpp"
 
-namespace smac_planner
+namespace nav2_smac_planner
 {
 
 /**
- * @class smac_planner::NodeBasic
+ * @class nav2_smac_planner::NodeBasic
  * @brief NodeBasic implementation for priority queue insertion
  */
 template<typename NodeT>
@@ -45,7 +45,7 @@ class NodeBasic
 {
 public:
   /**
-   * @brief A constructor for smac_planner::NodeBasic
+   * @brief A constructor for nav2_smac_planner::NodeBasic
    * @param cost_in The costmap cost at this node
    * @param index The index of this node for self-reference
    */
@@ -63,6 +63,6 @@ public:
 template class NodeBasic<Node2D>;
 template class NodeBasic<NodeSE2>;
 
-}  // namespace smac_planner
+}  // namespace nav2_smac_planner
 
-#endif  // SMAC_PLANNER__NODE_BASIC_HPP_
+#endif  // NAV2_SMAC_PLANNER__NODE_BASIC_HPP_
diff --git a/smac_planner/include/smac_planner/node_se2.hpp b/nav2_smac_planner/include/nav2_smac_planner/node_se2.hpp
similarity index 90%
rename from smac_planner/include/smac_planner/node_se2.hpp
rename to nav2_smac_planner/include/nav2_smac_planner/node_se2.hpp
index 4fb1ae57..4ea431c1 100644
--- a/smac_planner/include/smac_planner/node_se2.hpp
+++ b/nav2_smac_planner/include/nav2_smac_planner/node_se2.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License. Reserved.
 
-#ifndef SMAC_PLANNER__NODE_SE2_HPP_
-#define SMAC_PLANNER__NODE_SE2_HPP_
+#ifndef NAV2_SMAC_PLANNER__NODE_SE2_HPP_
+#define NAV2_SMAC_PLANNER__NODE_SE2_HPP_
 
 #include <math.h>
 #include <vector>
@@ -27,28 +27,28 @@
 
 #include "ompl/base/StateSpace.h"
 
-#include "smac_planner/constants.hpp"
-#include "smac_planner/types.hpp"
-#include "smac_planner/collision_checker.hpp"
+#include "nav2_smac_planner/constants.hpp"
+#include "nav2_smac_planner/types.hpp"
+#include "nav2_smac_planner/collision_checker.hpp"
 
-namespace smac_planner
+namespace nav2_smac_planner
 {
 
 // Need seperate pose struct for motion table operations
 
 /**
- * @struct smac_planner::MotionPose
+ * @struct nav2_smac_planner::MotionPose
  * @brief A struct for poses in motion primitives
  */
 struct MotionPose
 {
   /**
-   * @brief A constructor for smac_planner::MotionPose
+   * @brief A constructor for nav2_smac_planner::MotionPose
    */
   MotionPose() {}
 
   /**
-   * @brief A constructor for smac_planner::MotionPose
+   * @brief A constructor for nav2_smac_planner::MotionPose
    * @param x X pose
    * @param y Y pose
    * @param theta Angle of pose
@@ -68,13 +68,13 @@ typedef std::vector<MotionPose> MotionPoses;
 class NodeSE2;
 
 /**
- * @struct smac_planner::MotionTable
+ * @struct nav2_smac_planner::MotionTable
  * @brief A table of motion primitives and related functions
  */
 struct MotionTable
 {
   /**
-   * @brief A constructor for smac_planner::MotionTable
+   * @brief A constructor for nav2_smac_planner::MotionTable
    */
   MotionTable() {}
 
@@ -131,7 +131,7 @@ struct MotionTable
 };
 
 /**
- * @class smac_planner::NodeSE2
+ * @class nav2_smac_planner::NodeSE2
  * @brief NodeSE2 implementation for graph
  */
 class NodeSE2
@@ -141,18 +141,18 @@ public:
   typedef std::unique_ptr<std::vector<NodeSE2>> Graph;
   typedef std::vector<NodePtr> NodeVector;
   /**
-   * @class smac_planner::NodeSE2::Coordinates
+   * @class nav2_smac_planner::NodeSE2::Coordinates
    * @brief NodeSE2 implementation of coordinate structure
    */
   struct Coordinates
   {
     /**
-     * @brief A constructor for smac_planner::NodeSE2::Coordinates
+     * @brief A constructor for nav2_smac_planner::NodeSE2::Coordinates
      */
     Coordinates() {}
 
     /**
-     * @brief A constructor for smac_planner::NodeSE2::Coordinates
+     * @brief A constructor for nav2_smac_planner::NodeSE2::Coordinates
      * @param x_in X coordinate
      * @param y_in Y coordinate
      * @param theta_in Theta coordinate
@@ -167,13 +167,13 @@ public:
   typedef std::vector<Coordinates> CoordinateVector;
 
   /**
-   * @brief A constructor for smac_planner::NodeSE2
+   * @brief A constructor for nav2_smac_planner::NodeSE2
    * @param index The index of this node for self-reference
    */
   explicit NodeSE2(const unsigned int index);
 
   /**
-   * @brief A destructor for smac_planner::NodeSE2
+   * @brief A destructor for nav2_smac_planner::NodeSE2
    */
   ~NodeSE2();
 
@@ -398,7 +398,7 @@ public:
    */
   static void getNeighbors(
     const NodePtr & node,
-    std::function<bool(const unsigned int &, smac_planner::NodeSE2 * &)> & validity_checker,
+    std::function<bool(const unsigned int &, nav2_smac_planner::NodeSE2 * &)> & validity_checker,
     GridCollisionChecker collision_checker,
     const bool & traverse_unknown,
     NodeVector & neighbors);
@@ -418,6 +418,6 @@ private:
   static std::vector<unsigned int> _wavefront_heuristic;
 };
 
-}  // namespace smac_planner
+}  // namespace nav2_smac_planner
 
-#endif  // SMAC_PLANNER__NODE_SE2_HPP_
+#endif  // NAV2_SMAC_PLANNER__NODE_SE2_HPP_
diff --git a/smac_planner/include/smac_planner/options.hpp b/nav2_smac_planner/include/nav2_smac_planner/options.hpp
similarity index 95%
rename from smac_planner/include/smac_planner/options.hpp
rename to nav2_smac_planner/include/nav2_smac_planner/options.hpp
index 01951d89..159ab8b5 100644
--- a/smac_planner/include/smac_planner/options.hpp
+++ b/nav2_smac_planner/include/nav2_smac_planner/options.hpp
@@ -12,24 +12,24 @@
 // See the License for the specific language governing permissions and
 // limitations under the License. Reserved.
 
-#ifndef SMAC_PLANNER__OPTIONS_HPP_
-#define SMAC_PLANNER__OPTIONS_HPP_
+#ifndef NAV2_SMAC_PLANNER__OPTIONS_HPP_
+#define NAV2_SMAC_PLANNER__OPTIONS_HPP_
 
 #include <string>
 #include "rclcpp_lifecycle/lifecycle_node.hpp"
 #include "nav2_util/node_utils.hpp"
 
-namespace smac_planner
+namespace nav2_smac_planner
 {
 
 /**
- * @struct smac_planner::SmootherParams
+ * @struct nav2_smac_planner::SmootherParams
  * @brief Parameters for the smoother cost function
  */
 struct SmootherParams
 {
   /**
-   * @brief A constructor for smac_planner::SmootherParams
+   * @brief A constructor for nav2_smac_planner::SmootherParams
    */
   SmootherParams()
   {
@@ -72,7 +72,7 @@ struct SmootherParams
 };
 
 /**
- * @struct smac_planner::OptimizerParams
+ * @struct nav2_smac_planner::OptimizerParams
  * @brief Parameters for the ceres optimizer
  */
 struct OptimizerParams
@@ -202,6 +202,6 @@ struct OptimizerParams
   AdvancedParams advanced;
 };
 
-}  // namespace smac_planner
+}  // namespace nav2_smac_planner
 
-#endif  // SMAC_PLANNER__OPTIONS_HPP_
+#endif  // NAV2_SMAC_PLANNER__OPTIONS_HPP_
diff --git a/smac_planner/include/smac_planner/smac_planner.hpp b/nav2_smac_planner/include/nav2_smac_planner/smac_planner.hpp
similarity index 91%
rename from smac_planner/include/smac_planner/smac_planner.hpp
rename to nav2_smac_planner/include/nav2_smac_planner/smac_planner.hpp
index 7617c481..0f49dc7a 100644
--- a/smac_planner/include/smac_planner/smac_planner.hpp
+++ b/nav2_smac_planner/include/nav2_smac_planner/smac_planner.hpp
@@ -12,16 +12,16 @@
 // See the License for the specific language governing permissions and
 // limitations under the License. Reserved.
 
-#ifndef SMAC_PLANNER__SMAC_PLANNER_HPP_
-#define SMAC_PLANNER__SMAC_PLANNER_HPP_
+#ifndef NAV2_SMAC_PLANNER__SMAC_PLANNER_HPP_
+#define NAV2_SMAC_PLANNER__SMAC_PLANNER_HPP_
 
 #include <memory>
 #include <vector>
 #include <string>
 
-#include "smac_planner/a_star.hpp"
-#include "smac_planner/smoother.hpp"
-#include "smac_planner/costmap_downsampler.hpp"
+#include "nav2_smac_planner/a_star.hpp"
+#include "nav2_smac_planner/smoother.hpp"
+#include "nav2_smac_planner/costmap_downsampler.hpp"
 #include "nav_msgs/msg/occupancy_grid.hpp"
 #include "nav2_core/global_planner.hpp"
 #include "nav_msgs/msg/path.hpp"
@@ -32,7 +32,7 @@
 #include "nav2_util/node_utils.hpp"
 #include "tf2/utils.h"
 
-namespace smac_planner
+namespace nav2_smac_planner
 {
 
 class SmacPlanner : public nav2_core::GlobalPlanner
@@ -127,6 +127,6 @@ protected:
   double _max_planning_time;
 };
 
-}  // namespace smac_planner
+}  // namespace nav2_smac_planner
 
-#endif  // SMAC_PLANNER__SMAC_PLANNER_HPP_
+#endif  // NAV2_SMAC_PLANNER__SMAC_PLANNER_HPP_
diff --git a/smac_planner/include/smac_planner/smac_planner_2d.hpp b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_2d.hpp
similarity index 90%
rename from smac_planner/include/smac_planner/smac_planner_2d.hpp
rename to nav2_smac_planner/include/nav2_smac_planner/smac_planner_2d.hpp
index 3b6331fd..31f10496 100644
--- a/smac_planner/include/smac_planner/smac_planner_2d.hpp
+++ b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_2d.hpp
@@ -12,16 +12,16 @@
 // See the License for the specific language governing permissions and
 // limitations under the License. Reserved.
 
-#ifndef SMAC_PLANNER__SMAC_PLANNER_2D_HPP_
-#define SMAC_PLANNER__SMAC_PLANNER_2D_HPP_
+#ifndef NAV2_SMAC_PLANNER__SMAC_PLANNER_2D_HPP_
+#define NAV2_SMAC_PLANNER__SMAC_PLANNER_2D_HPP_
 
 #include <memory>
 #include <vector>
 #include <string>
 
-#include "smac_planner/a_star.hpp"
-#include "smac_planner/smoother.hpp"
-#include "smac_planner/costmap_downsampler.hpp"
+#include "nav2_smac_planner/a_star.hpp"
+#include "nav2_smac_planner/smoother.hpp"
+#include "nav2_smac_planner/costmap_downsampler.hpp"
 #include "nav_msgs/msg/occupancy_grid.hpp"
 #include "nav2_core/global_planner.hpp"
 #include "nav_msgs/msg/path.hpp"
@@ -32,7 +32,7 @@
 #include "nav2_util/node_utils.hpp"
 #include "tf2/utils.h"
 
-namespace smac_planner
+namespace nav2_smac_planner
 {
 
 class SmacPlanner2D : public nav2_core::GlobalPlanner
@@ -118,6 +118,6 @@ protected:
   double _max_planning_time;
 };
 
-}  // namespace smac_planner
+}  // namespace nav2_smac_planner
 
-#endif  // SMAC_PLANNER__SMAC_PLANNER_2D_HPP_
+#endif  // NAV2_SMAC_PLANNER__SMAC_PLANNER_2D_HPP_
diff --git a/smac_planner/include/smac_planner/smoother.hpp b/nav2_smac_planner/include/nav2_smac_planner/smoother.hpp
similarity index 90%
rename from smac_planner/include/smac_planner/smoother.hpp
rename to nav2_smac_planner/include/nav2_smac_planner/smoother.hpp
index 756e6b57..b3b0fe82 100644
--- a/smac_planner/include/smac_planner/smoother.hpp
+++ b/nav2_smac_planner/include/nav2_smac_planner/smoother.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License. Reserved.
 
-#ifndef SMAC_PLANNER__SMOOTHER_HPP_
-#define SMAC_PLANNER__SMOOTHER_HPP_
+#ifndef NAV2_SMAC_PLANNER__SMOOTHER_HPP_
+#define NAV2_SMAC_PLANNER__SMOOTHER_HPP_
 
 #include <cmath>
 #include <vector>
@@ -22,29 +22,29 @@
 #include <queue>
 #include <utility>
 
-#include "smac_planner/types.hpp"
-#include "smac_planner/smoother_cost_function.hpp"
+#include "nav2_smac_planner/types.hpp"
+#include "nav2_smac_planner/smoother_cost_function.hpp"
 
 #include "ceres/ceres.h"
 #include "Eigen/Core"
 
-namespace smac_planner
+namespace nav2_smac_planner
 {
 
 /**
- * @class smac_planner::Smoother
+ * @class nav2_smac_planner::Smoother
  * @brief A Conjugate Gradient 2D path smoother implementation
  */
 class Smoother
 {
 public:
   /**
-   * @brief A constructor for smac_planner::Smoother
+   * @brief A constructor for nav2_smac_planner::Smoother
    */
   Smoother() {}
 
   /**
-   * @brief A destructor for smac_planner::Smoother
+   * @brief A destructor for nav2_smac_planner::Smoother
    */
   ~Smoother() {}
 
@@ -136,6 +136,6 @@ private:
   ceres::GradientProblemSolver::Options _options;
 };
 
-}  // namespace smac_planner
+}  // namespace nav2_smac_planner
 
-#endif  // SMAC_PLANNER__SMOOTHER_HPP_
+#endif  // NAV2_SMAC_PLANNER__SMOOTHER_HPP_
diff --git a/smac_planner/include/smac_planner/smoother_cost_function.hpp b/nav2_smac_planner/include/nav2_smac_planner/smoother_cost_function.hpp
similarity index 96%
rename from smac_planner/include/smac_planner/smoother_cost_function.hpp
rename to nav2_smac_planner/include/nav2_smac_planner/smoother_cost_function.hpp
index 82d1c88a..eec8c61b 100644
--- a/smac_planner/include/smac_planner/smoother_cost_function.hpp
+++ b/nav2_smac_planner/include/nav2_smac_planner/smoother_cost_function.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License. Reserved.
 
-#ifndef SMAC_PLANNER__SMOOTHER_COST_FUNCTION_HPP_
-#define SMAC_PLANNER__SMOOTHER_COST_FUNCTION_HPP_
+#ifndef NAV2_SMAC_PLANNER__SMOOTHER_COST_FUNCTION_HPP_
+#define NAV2_SMAC_PLANNER__SMOOTHER_COST_FUNCTION_HPP_
 
 #include <cmath>
 #include <vector>
@@ -25,17 +25,17 @@
 
 #include "ceres/ceres.h"
 #include "Eigen/Core"
-#include "smac_planner/types.hpp"
+#include "nav2_smac_planner/types.hpp"
 #include "nav2_costmap_2d/costmap_2d.hpp"
-#include "smac_planner/options.hpp"
+#include "nav2_smac_planner/options.hpp"
 
 #define EPSILON 0.0001
 
-namespace smac_planner
+namespace nav2_smac_planner
 {
 
 /**
- * @struct smac_planner::UnconstrainedSmootherCostFunction
+ * @struct nav2_smac_planner::UnconstrainedSmootherCostFunction
  * @brief Cost function for path smoothing with multiple terms
  * including curvature, smoothness, collision, and avoid obstacles.
  */
@@ -43,7 +43,7 @@ class UnconstrainedSmootherCostFunction : public ceres::FirstOrderFunction
 {
 public:
   /**
-   * @brief A constructor for smac_planner::UnconstrainedSmootherCostFunction
+   * @brief A constructor for nav2_smac_planner::UnconstrainedSmootherCostFunction
    * @param original_path Original unsmoothed path to smooth
    * @param costmap A costmap to get values for collision and obstacle avoidance
    */
@@ -65,7 +65,7 @@ public:
   struct CurvatureComputations
   {
     /**
-     * @brief A constructor for smac_planner::CurvatureComputations
+     * @brief A constructor for nav2_smac_planner::CurvatureComputations
      */
     CurvatureComputations()
     {
@@ -510,6 +510,6 @@ protected:
   SmootherParams _params;
 };
 
-}  // namespace smac_planner
+}  // namespace nav2_smac_planner
 
-#endif  // SMAC_PLANNER__SMOOTHER_COST_FUNCTION_HPP_
+#endif  // NAV2_SMAC_PLANNER__SMOOTHER_COST_FUNCTION_HPP_
diff --git a/smac_planner/include/smac_planner/types.hpp b/nav2_smac_planner/include/nav2_smac_planner/types.hpp
similarity index 81%
rename from smac_planner/include/smac_planner/types.hpp
rename to nav2_smac_planner/include/nav2_smac_planner/types.hpp
index e39564d3..eeeb9218 100644
--- a/smac_planner/include/smac_planner/types.hpp
+++ b/nav2_smac_planner/include/nav2_smac_planner/types.hpp
@@ -12,19 +12,19 @@
 // See the License for the specific language governing permissions and
 // limitations under the License. Reserved.
 
-#ifndef SMAC_PLANNER__TYPES_HPP_
-#define SMAC_PLANNER__TYPES_HPP_
+#ifndef NAV2_SMAC_PLANNER__TYPES_HPP_
+#define NAV2_SMAC_PLANNER__TYPES_HPP_
 
 #include <vector>
 #include <utility>
 
-namespace smac_planner
+namespace nav2_smac_planner
 {
 
 typedef std::pair<float, unsigned int> NodeHeuristicPair;
 
 /**
- * @struct smac_planner::SearchInfo
+ * @struct nav2_smac_planner::SearchInfo
  * @brief Search properties and penalties
  */
 struct SearchInfo
@@ -37,6 +37,6 @@ struct SearchInfo
   float analytic_expansion_ratio;
 };
 
-}  // namespace smac_planner
+}  // namespace nav2_smac_planner
 
-#endif  // SMAC_PLANNER__TYPES_HPP_
+#endif  // NAV2_SMAC_PLANNER__TYPES_HPP_
diff --git a/smac_planner/package.xml b/nav2_smac_planner/package.xml
similarity index 97%
rename from smac_planner/package.xml
rename to nav2_smac_planner/package.xml
index 8de16907..a266e7d5 100644
--- a/smac_planner/package.xml
+++ b/nav2_smac_planner/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
-  <name>smac_planner</name>
+  <name>nav2_smac_planner</name>
   <version>0.3.0</version>
   <description>Smac global planning plugin</description>
   <maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>
diff --git a/nav2_smac_planner/smac_plugin.xml b/nav2_smac_planner/smac_plugin.xml
new file mode 100644
index 00000000..c28cb32c
--- /dev/null
+++ b/nav2_smac_planner/smac_plugin.xml
@@ -0,0 +1,5 @@
+<library path="nav2_smac_planner">
+	<class name="nav2_smac_planner/SmacPlanner" type="nav2_smac_planner::SmacPlanner" base_class_type="nav2_core::GlobalPlanner">
+	  <description>SE2 version of the SMAC planner</description>
+	</class>
+</library>
diff --git a/nav2_smac_planner/smac_plugin_2d.xml b/nav2_smac_planner/smac_plugin_2d.xml
new file mode 100644
index 00000000..aab22602
--- /dev/null
+++ b/nav2_smac_planner/smac_plugin_2d.xml
@@ -0,0 +1,5 @@
+<library path="nav2_smac_planner_2d">
+  <class name="nav2_smac_planner/SmacPlanner2D" type="nav2_smac_planner::SmacPlanner2D" base_class_type="nav2_core::GlobalPlanner">
+    <description>2D version of the SMAC Planner</description>
+  </class>
+</library>
diff --git a/smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp
similarity index 99%
rename from smac_planner/src/a_star.cpp
rename to nav2_smac_planner/src/a_star.cpp
index fae4694f..788f71cb 100644
--- a/smac_planner/src/a_star.cpp
+++ b/nav2_smac_planner/src/a_star.cpp
@@ -30,10 +30,10 @@
 #include <utility>
 #include <vector>
 
-#include "smac_planner/a_star.hpp"
+#include "nav2_smac_planner/a_star.hpp"
 using namespace std::chrono;  // NOLINT
 
-namespace smac_planner
+namespace nav2_smac_planner
 {
 
 template<typename NodeT>
@@ -648,4 +648,4 @@ typename AStarAlgorithm<NodeT>::NodePtr AStarAlgorithm<NodeT>::tryAnalyticExpans
 template class AStarAlgorithm<Node2D>;
 template class AStarAlgorithm<NodeSE2>;
 
-}  // namespace smac_planner
+}  // namespace nav2_smac_planner
diff --git a/smac_planner/src/costmap_downsampler.cpp b/nav2_smac_planner/src/costmap_downsampler.cpp
similarity index 97%
rename from smac_planner/src/costmap_downsampler.cpp
rename to nav2_smac_planner/src/costmap_downsampler.cpp
index 4e4824f4..c7225234 100644
--- a/smac_planner/src/costmap_downsampler.cpp
+++ b/nav2_smac_planner/src/costmap_downsampler.cpp
@@ -13,13 +13,13 @@
 // See the License for the specific language governing permissions and
 // limitations under the License. Reserved.
 
-#include "smac_planner/costmap_downsampler.hpp"
+#include "nav2_smac_planner/costmap_downsampler.hpp"
 
 #include <string>
 #include <memory>
 #include <algorithm>
 
-namespace smac_planner
+namespace nav2_smac_planner
 {
 
 CostmapDownsampler::CostmapDownsampler()
@@ -139,4 +139,4 @@ void CostmapDownsampler::setCostOfCell(
   _downsampled_costmap->setCost(new_mx, new_my, cost);
 }
 
-}  // namespace smac_planner
+}  // namespace nav2_smac_planner
diff --git a/smac_planner/src/node_2d.cpp b/nav2_smac_planner/src/node_2d.cpp
similarity index 96%
rename from smac_planner/src/node_2d.cpp
rename to nav2_smac_planner/src/node_2d.cpp
index b6afd5b9..cd8a2238 100644
--- a/smac_planner/src/node_2d.cpp
+++ b/nav2_smac_planner/src/node_2d.cpp
@@ -13,12 +13,12 @@
 // See the License for the specific language governing permissions and
 // limitations under the License. Reserved.
 
-#include "smac_planner/node_2d.hpp"
+#include "nav2_smac_planner/node_2d.hpp"
 
 #include <vector>
 #include <limits>
 
-namespace smac_planner
+namespace nav2_smac_planner
 {
 
 // defining static member for all instance to share
@@ -118,7 +118,7 @@ void Node2D::initNeighborhood(
 
 void Node2D::getNeighbors(
   NodePtr & node,
-  std::function<bool(const unsigned int &, smac_planner::Node2D * &)> & NeighborGetter,
+  std::function<bool(const unsigned int &, nav2_smac_planner::Node2D * &)> & NeighborGetter,
   GridCollisionChecker collision_checker,
   const bool & traverse_unknown,
   NodeVector & neighbors)
@@ -148,4 +148,4 @@ void Node2D::getNeighbors(
   }
 }
 
-}  // namespace smac_planner
+}  // namespace nav2_smac_planner
diff --git a/smac_planner/src/node_se2.cpp b/nav2_smac_planner/src/node_se2.cpp
similarity index 98%
rename from smac_planner/src/node_se2.cpp
rename to nav2_smac_planner/src/node_se2.cpp
index 9764df0d..9bc2516d 100644
--- a/smac_planner/src/node_se2.cpp
+++ b/nav2_smac_planner/src/node_se2.cpp
@@ -25,11 +25,11 @@
 #include "ompl/base/spaces/DubinsStateSpace.h"
 #include "ompl/base/spaces/ReedsSheppStateSpace.h"
 
-#include "smac_planner/node_se2.hpp"
+#include "nav2_smac_planner/node_se2.hpp"
 
 using namespace std::chrono;  // NOLINT
 
-namespace smac_planner
+namespace nav2_smac_planner
 {
 
 // defining static member for all instance to share
@@ -393,7 +393,7 @@ void NodeSE2::computeWavefrontHeuristic(
 
 void NodeSE2::getNeighbors(
   const NodePtr & node,
-  std::function<bool(const unsigned int &, smac_planner::NodeSE2 * &)> & NeighborGetter,
+  std::function<bool(const unsigned int &, nav2_smac_planner::NodeSE2 * &)> & NeighborGetter,
   GridCollisionChecker collision_checker,
   const bool & traverse_unknown,
   NodeVector & neighbors)
@@ -429,4 +429,4 @@ void NodeSE2::getNeighbors(
   }
 }
 
-}  // namespace smac_planner
+}  // namespace nav2_smac_planner
diff --git a/smac_planner/src/smac_planner.cpp b/nav2_smac_planner/src/smac_planner.cpp
similarity index 98%
rename from smac_planner/src/smac_planner.cpp
rename to nav2_smac_planner/src/smac_planner.cpp
index 8d56f49a..b22b61a2 100644
--- a/smac_planner/src/smac_planner.cpp
+++ b/nav2_smac_planner/src/smac_planner.cpp
@@ -19,11 +19,11 @@
 #include <limits>
 
 #include "Eigen/Core"
-#include "smac_planner/smac_planner.hpp"
+#include "nav2_smac_planner/smac_planner.hpp"
 
 #define BENCHMARK_TESTING
 
-namespace smac_planner
+namespace nav2_smac_planner
 {
 
 using namespace std::chrono;  // NOLINT
@@ -382,7 +382,7 @@ geometry_msgs::msg::Quaternion SmacPlanner::getWorldOrientation(const float & th
   return tf2::toMsg(q);
 }
 
-}  // namespace smac_planner
+}  // namespace nav2_smac_planner
 
 #include "pluginlib/class_list_macros.hpp"
-PLUGINLIB_EXPORT_CLASS(smac_planner::SmacPlanner, nav2_core::GlobalPlanner)
+PLUGINLIB_EXPORT_CLASS(nav2_smac_planner::SmacPlanner, nav2_core::GlobalPlanner)
diff --git a/smac_planner/src/smac_planner_2d.cpp b/nav2_smac_planner/src/smac_planner_2d.cpp
similarity index 98%
rename from smac_planner/src/smac_planner_2d.cpp
rename to nav2_smac_planner/src/smac_planner_2d.cpp
index 820218f5..f8c82f08 100644
--- a/smac_planner/src/smac_planner_2d.cpp
+++ b/nav2_smac_planner/src/smac_planner_2d.cpp
@@ -18,11 +18,11 @@
 #include <limits>
 #include <algorithm>
 
-#include "smac_planner/smac_planner_2d.hpp"
+#include "nav2_smac_planner/smac_planner_2d.hpp"
 
 // #define BENCHMARK_TESTING
 
-namespace smac_planner
+namespace nav2_smac_planner
 {
 using namespace std::chrono;  // NOLINT
 
@@ -336,7 +336,7 @@ Eigen::Vector2d SmacPlanner2D::getWorldCoords(
   return Eigen::Vector2d(world_x, world_y);
 }
 
-}  // namespace smac_planner
+}  // namespace nav2_smac_planner
 
 #include "pluginlib/class_list_macros.hpp"
-PLUGINLIB_EXPORT_CLASS(smac_planner::SmacPlanner2D, nav2_core::GlobalPlanner)
+PLUGINLIB_EXPORT_CLASS(nav2_smac_planner::SmacPlanner2D, nav2_core::GlobalPlanner)
diff --git a/smac_planner/test/CMakeLists.txt b/nav2_smac_planner/test/CMakeLists.txt
similarity index 100%
rename from smac_planner/test/CMakeLists.txt
rename to nav2_smac_planner/test/CMakeLists.txt
diff --git a/smac_planner/test/deprecated_upsampler/upsampler.hpp b/nav2_smac_planner/test/deprecated_upsampler/upsampler.hpp
similarity index 94%
rename from smac_planner/test/deprecated_upsampler/upsampler.hpp
rename to nav2_smac_planner/test/deprecated_upsampler/upsampler.hpp
index 8bfc36c0..84ba625f 100644
--- a/smac_planner/test/deprecated_upsampler/upsampler.hpp
+++ b/nav2_smac_planner/test/deprecated_upsampler/upsampler.hpp
@@ -23,30 +23,30 @@
 #include <algorithm>
 #include <utility>
 
-#include "smac_planner/types.hpp"
-#include "smac_planner/upsampler_cost_function.hpp"
-#include "smac_planner/upsampler_cost_function_nlls.hpp"
+#include "nav2_smac_planner/types.hpp"
+#include "nav2_smac_planner/upsampler_cost_function.hpp"
+#include "nav2_smac_planner/upsampler_cost_function_nlls.hpp"
 
 #include "ceres/ceres.h"
 #include "Eigen/Core"
 
-namespace smac_planner
+namespace nav2_smac_planner
 {
 
 /**
- * @class smac_planner::Upsampler
+ * @class nav2_smac_planner::Upsampler
  * @brief A Conjugate Gradient 2D path upsampler implementation
  */
 class Upsampler
 {
 public:
   /**
-   * @brief A constructor for smac_planner::Upsampler
+   * @brief A constructor for nav2_smac_planner::Upsampler
    */
   Upsampler() {}
 
   /**
-   * @brief A destructor for smac_planner::Upsampler
+   * @brief A destructor for nav2_smac_planner::Upsampler
    */
   ~Upsampler() {}
 
@@ -208,6 +208,6 @@ private:
   ceres::GradientProblemSolver::Options _options;
 };
 
-}  // namespace smac_planner
+}  // namespace nav2_smac_planner
 
 #endif  // DEPRECATED_UPSAMPLER__UPSAMPLER_HPP_
diff --git a/smac_planner/test/deprecated_upsampler/upsampler_cost_function.hpp b/nav2_smac_planner/test/deprecated_upsampler/upsampler_cost_function.hpp
similarity index 97%
rename from smac_planner/test/deprecated_upsampler/upsampler_cost_function.hpp
rename to nav2_smac_planner/test/deprecated_upsampler/upsampler_cost_function.hpp
index cf84acf6..44df1c3f 100644
--- a/smac_planner/test/deprecated_upsampler/upsampler_cost_function.hpp
+++ b/nav2_smac_planner/test/deprecated_upsampler/upsampler_cost_function.hpp
@@ -25,15 +25,15 @@
 
 #include "ceres/ceres.h"
 #include "Eigen/Core"
-#include "smac_planner/types.hpp"
-#include "smac_planner/options.hpp"
+#include "nav2_smac_planner/types.hpp"
+#include "nav2_smac_planner/options.hpp"
 
 #define EPSILON 0.0001
 
-namespace smac_planner
+namespace nav2_smac_planner
 {
 /**
- * @struct smac_planner::UpsamplerCostFunction
+ * @struct nav2_smac_planner::UpsamplerCostFunction
  * @brief Cost function for path upsampling with multiple terms using unconstrained
  * optimization including curvature, smoothness, collision, and avoid obstacles.
  */
@@ -41,7 +41,7 @@ class UpsamplerCostFunction : public ceres::FirstOrderFunction
 {
 public:
   /**
-   * @brief A constructor for smac_planner::UpsamplerCostFunction
+   * @brief A constructor for nav2_smac_planner::UpsamplerCostFunction
    * @param num_points Number of path points to consider
    */
   UpsamplerCostFunction(
@@ -63,7 +63,7 @@ public:
   struct CurvatureComputations
   {
     /**
-     * @brief A constructor for smac_planner::CurvatureComputations
+     * @brief A constructor for nav2_smac_planner::CurvatureComputations
      */
     CurvatureComputations()
     {
@@ -361,6 +361,6 @@ protected:
   std::vector<Eigen::Vector2d> _path;
 };
 
-}  // namespace smac_planner
+}  // namespace nav2_smac_planner
 
 #endif  // DEPRECATED_UPSAMPLER__UPSAMPLER_COST_FUNCTION_HPP_
diff --git a/smac_planner/test/deprecated_upsampler/upsampler_cost_function_nlls.hpp b/nav2_smac_planner/test/deprecated_upsampler/upsampler_cost_function_nlls.hpp
similarity index 96%
rename from smac_planner/test/deprecated_upsampler/upsampler_cost_function_nlls.hpp
rename to nav2_smac_planner/test/deprecated_upsampler/upsampler_cost_function_nlls.hpp
index a6fd595f..17204694 100644
--- a/smac_planner/test/deprecated_upsampler/upsampler_cost_function_nlls.hpp
+++ b/nav2_smac_planner/test/deprecated_upsampler/upsampler_cost_function_nlls.hpp
@@ -25,15 +25,15 @@
 
 #include "ceres/ceres.h"
 #include "Eigen/Core"
-#include "smac_planner/types.hpp"
-#include "smac_planner/options.hpp"
+#include "nav2_smac_planner/types.hpp"
+#include "nav2_smac_planner/options.hpp"
 
 #define EPSILON 0.0001
 
-namespace smac_planner
+namespace nav2_smac_planner
 {
 /**
- * @struct smac_planner::UpsamplerConstrainedCostFunction
+ * @struct nav2_smac_planner::UpsamplerConstrainedCostFunction
  * @brief Cost function for path upsampling with multiple terms using NLLS
  * including curvature, smoothness, collision, and avoid obstacles.
  */
@@ -41,7 +41,7 @@ class UpsamplerConstrainedCostFunction : public ceres::SizedCostFunction<1, 1, 1
 {
 public:
   /**
-   * @brief A constructor for smac_planner::UpsamplerConstrainedCostFunction
+   * @brief A constructor for nav2_smac_planner::UpsamplerConstrainedCostFunction
    * @param num_points Number of path points to consider
    */
   UpsamplerConstrainedCostFunction(
@@ -63,7 +63,7 @@ public:
   struct CurvatureComputations
   {
     /**
-     * @brief A constructor for smac_planner::CurvatureComputations
+     * @brief A constructor for nav2_smac_planner::CurvatureComputations
      */
     CurvatureComputations()
     {
@@ -329,6 +329,6 @@ protected:
   int index;
 };
 
-}  // namespace smac_planner
+}  // namespace nav2_smac_planner
 
 #endif  // DEPRECATED_UPSAMPLER__UPSAMPLER_COST_FUNCTION_NLLS_HPP_
diff --git a/smac_planner/test/path.png b/nav2_smac_planner/test/path.png
similarity index 100%
rename from smac_planner/test/path.png
rename to nav2_smac_planner/test/path.png
diff --git a/smac_planner/test/test_a_star.cpp b/nav2_smac_planner/test/test_a_star.cpp
similarity index 73%
rename from smac_planner/test/test_a_star.cpp
rename to nav2_smac_planner/test/test_a_star.cpp
index 091ee0cf..721eee47 100644
--- a/smac_planner/test/test_a_star.cpp
+++ b/nav2_smac_planner/test/test_a_star.cpp
@@ -22,9 +22,9 @@
 #include "nav2_costmap_2d/costmap_2d.hpp"
 #include "nav2_costmap_2d/costmap_subscriber.hpp"
 #include "nav2_util/lifecycle_node.hpp"
-#include "smac_planner/node_se2.hpp"
-#include "smac_planner/a_star.hpp"
-#include "smac_planner/collision_checker.hpp"
+#include "nav2_smac_planner/node_se2.hpp"
+#include "nav2_smac_planner/a_star.hpp"
+#include "nav2_smac_planner/collision_checker.hpp"
 
 class RclCppFixture
 {
@@ -36,8 +36,9 @@ RclCppFixture g_rclcppfixture;
 
 TEST(AStarTest, test_a_star_2d)
 {
-  smac_planner::SearchInfo info;
-  smac_planner::AStarAlgorithm<smac_planner::Node2D> a_star(smac_planner::MotionModel::MOORE, info);
+  nav2_smac_planner::SearchInfo info;
+  nav2_smac_planner::AStarAlgorithm<nav2_smac_planner::Node2D> a_star(
+    nav2_smac_planner::MotionModel::MOORE, info);
   int max_iterations = 10000;
   float tolerance = 0.0;
   float some_tolerance = 20.0;
@@ -60,7 +61,7 @@ TEST(AStarTest, test_a_star_2d)
   a_star.createGraph(costmapA->getSizeInCellsX(), costmapA->getSizeInCellsY(), 1, costmapA);
   a_star.setStart(20u, 20u, 0);
   a_star.setGoal(80u, 80u, 0);
-  smac_planner::Node2D::CoordinateVector path;
+  nav2_smac_planner::Node2D::CoordinateVector path;
   EXPECT_TRUE(a_star.createPath(path, num_it, tolerance));
   EXPECT_EQ(num_it, 556);
 
@@ -79,8 +80,8 @@ TEST(AStarTest, test_a_star_2d)
 
   path.clear();
   // failure cases with invalid inputs
-  smac_planner::AStarAlgorithm<smac_planner::Node2D> a_star_2(
-    smac_planner::MotionModel::VON_NEUMANN, info);
+  nav2_smac_planner::AStarAlgorithm<nav2_smac_planner::Node2D> a_star_2(
+    nav2_smac_planner::MotionModel::VON_NEUMANN, info);
   a_star_2.initialize(false, max_iterations, it_on_approach);
   a_star_2.setFootprint(nav2_costmap_2d::Footprint(), true);
   num_it = 0;
@@ -119,14 +120,14 @@ TEST(AStarTest, test_a_star_2d)
 
 TEST(AStarTest, test_a_star_se2)
 {
-  smac_planner::SearchInfo info;
+  nav2_smac_planner::SearchInfo info;
   info.change_penalty = 1.2;
   info.non_straight_penalty = 1.4;
   info.reverse_penalty = 2.1;
   info.minimum_turning_radius = 2.0;  // in grid coordinates
   unsigned int size_theta = 72;
-  smac_planner::AStarAlgorithm<smac_planner::NodeSE2> a_star(
-    smac_planner::MotionModel::DUBIN, info);
+  nav2_smac_planner::AStarAlgorithm<nav2_smac_planner::NodeSE2> a_star(
+    nav2_smac_planner::MotionModel::DUBIN, info);
   int max_iterations = 10000;
   float tolerance = 10.0;
   int it_on_approach = 10;
@@ -149,7 +150,7 @@ TEST(AStarTest, test_a_star_se2)
     costmapA->getSizeInCellsX(), costmapA->getSizeInCellsY(), size_theta, costmapA);
   a_star.setStart(10u, 10u, 0u);
   a_star.setGoal(80u, 80u, 40u);
-  smac_planner::NodeSE2::CoordinateVector path;
+  nav2_smac_planner::NodeSE2::CoordinateVector path;
   EXPECT_TRUE(a_star.createPath(path, num_it, tolerance));
 
   // check path is the right size and collision free
@@ -164,20 +165,24 @@ TEST(AStarTest, test_a_star_se2)
 
 TEST(AStarTest, test_constants)
 {
-  smac_planner::MotionModel mm = smac_planner::MotionModel::UNKNOWN;  // unknown
-  EXPECT_EQ(smac_planner::toString(mm), std::string("Unknown"));
-  mm = smac_planner::MotionModel::VON_NEUMANN;  // vonneumann
-  EXPECT_EQ(smac_planner::toString(mm), std::string("Von Neumann"));
-  mm = smac_planner::MotionModel::MOORE;  // moore
-  EXPECT_EQ(smac_planner::toString(mm), std::string("Moore"));
-  mm = smac_planner::MotionModel::DUBIN;  // dubin
-  EXPECT_EQ(smac_planner::toString(mm), std::string("Dubin"));
-  mm = smac_planner::MotionModel::REEDS_SHEPP;  // reeds-shepp
-  EXPECT_EQ(smac_planner::toString(mm), std::string("Reeds-Shepp"));
-
-  EXPECT_EQ(smac_planner::fromString("VON_NEUMANN"), smac_planner::MotionModel::VON_NEUMANN);
-  EXPECT_EQ(smac_planner::fromString("MOORE"), smac_planner::MotionModel::MOORE);
-  EXPECT_EQ(smac_planner::fromString("DUBIN"), smac_planner::MotionModel::DUBIN);
-  EXPECT_EQ(smac_planner::fromString("REEDS_SHEPP"), smac_planner::MotionModel::REEDS_SHEPP);
-  EXPECT_EQ(smac_planner::fromString("NONE"), smac_planner::MotionModel::UNKNOWN);
+  nav2_smac_planner::MotionModel mm = nav2_smac_planner::MotionModel::UNKNOWN;  // unknown
+  EXPECT_EQ(nav2_smac_planner::toString(mm), std::string("Unknown"));
+  mm = nav2_smac_planner::MotionModel::VON_NEUMANN;  // vonneumann
+  EXPECT_EQ(nav2_smac_planner::toString(mm), std::string("Von Neumann"));
+  mm = nav2_smac_planner::MotionModel::MOORE;  // moore
+  EXPECT_EQ(nav2_smac_planner::toString(mm), std::string("Moore"));
+  mm = nav2_smac_planner::MotionModel::DUBIN;  // dubin
+  EXPECT_EQ(nav2_smac_planner::toString(mm), std::string("Dubin"));
+  mm = nav2_smac_planner::MotionModel::REEDS_SHEPP;  // reeds-shepp
+  EXPECT_EQ(nav2_smac_planner::toString(mm), std::string("Reeds-Shepp"));
+
+  EXPECT_EQ(
+    nav2_smac_planner::fromString(
+      "VON_NEUMANN"), nav2_smac_planner::MotionModel::VON_NEUMANN);
+  EXPECT_EQ(nav2_smac_planner::fromString("MOORE"), nav2_smac_planner::MotionModel::MOORE);
+  EXPECT_EQ(nav2_smac_planner::fromString("DUBIN"), nav2_smac_planner::MotionModel::DUBIN);
+  EXPECT_EQ(
+    nav2_smac_planner::fromString(
+      "REEDS_SHEPP"), nav2_smac_planner::MotionModel::REEDS_SHEPP);
+  EXPECT_EQ(nav2_smac_planner::fromString("NONE"), nav2_smac_planner::MotionModel::UNKNOWN);
 }
diff --git a/smac_planner/test/test_collision_checker.cpp b/nav2_smac_planner/test/test_collision_checker.cpp
similarity index 92%
rename from smac_planner/test/test_collision_checker.cpp
rename to nav2_smac_planner/test/test_collision_checker.cpp
index e6590d61..687b4b5c 100644
--- a/smac_planner/test/test_collision_checker.cpp
+++ b/nav2_smac_planner/test/test_collision_checker.cpp
@@ -18,7 +18,7 @@
 #include <memory>
 
 #include "gtest/gtest.h"
-#include "smac_planner/collision_checker.hpp"
+#include "nav2_smac_planner/collision_checker.hpp"
 
 using namespace nav2_costmap_2d;  // NOLINT
 
@@ -41,7 +41,7 @@ TEST(collision_footprint, test_basic)
 
   nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4};
 
-  smac_planner::GridCollisionChecker collision_checker(costmap_);
+  nav2_smac_planner::GridCollisionChecker collision_checker(costmap_);
   collision_checker.setFootprint(footprint, false /*use footprint*/);
   collision_checker.inCollision(5.0, 5.0, 0.0, false);
   float cost = collision_checker.getCost();
@@ -53,7 +53,7 @@ TEST(collision_footprint, test_point_cost)
 {
   nav2_costmap_2d::Costmap2D * costmap_ = new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0, 0, 0);
 
-  smac_planner::GridCollisionChecker collision_checker(costmap_);
+  nav2_smac_planner::GridCollisionChecker collision_checker(costmap_);
   nav2_costmap_2d::Footprint footprint;
   collision_checker.setFootprint(footprint, true /*radius / pointcose*/);
 
@@ -67,7 +67,7 @@ TEST(collision_footprint, test_world_to_map)
 {
   nav2_costmap_2d::Costmap2D * costmap_ = new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0, 0, 0);
 
-  smac_planner::GridCollisionChecker collision_checker(costmap_);
+  nav2_smac_planner::GridCollisionChecker collision_checker(costmap_);
   nav2_costmap_2d::Footprint footprint;
   collision_checker.setFootprint(footprint, true /*radius / point cost*/);
 
@@ -113,7 +113,7 @@ TEST(collision_footprint, test_footprint_at_pose_with_movement)
 
   nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4};
 
-  smac_planner::GridCollisionChecker collision_checker(costmap_);
+  nav2_smac_planner::GridCollisionChecker collision_checker(costmap_);
   collision_checker.setFootprint(footprint, false /*use footprint*/);
 
   collision_checker.inCollision(50, 50, 0.0, false);
@@ -154,7 +154,7 @@ TEST(collision_footprint, test_point_and_line_cost)
 
   nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4};
 
-  smac_planner::GridCollisionChecker collision_checker(costmap_);
+  nav2_smac_planner::GridCollisionChecker collision_checker(costmap_);
   collision_checker.setFootprint(footprint, false /*use footprint*/);
 
   collision_checker.inCollision(50, 50, 0.0, false);
diff --git a/smac_planner/test/test_costmap_downsampler.cpp b/nav2_smac_planner/test/test_costmap_downsampler.cpp
similarity index 95%
rename from smac_planner/test/test_costmap_downsampler.cpp
rename to nav2_smac_planner/test/test_costmap_downsampler.cpp
index 5353dcb3..1317c343 100644
--- a/smac_planner/test/test_costmap_downsampler.cpp
+++ b/nav2_smac_planner/test/test_costmap_downsampler.cpp
@@ -19,7 +19,7 @@
 #include "rclcpp/rclcpp.hpp"
 #include "nav2_costmap_2d/costmap_2d.hpp"
 #include "nav2_util/lifecycle_node.hpp"
-#include "smac_planner/costmap_downsampler.hpp"
+#include "nav2_smac_planner/costmap_downsampler.hpp"
 
 class RclCppFixture
 {
@@ -33,7 +33,7 @@ TEST(CostmapDownsampler, costmap_downsample_test)
 {
   nav2_util::LifecycleNode::SharedPtr node = std::make_shared<nav2_util::LifecycleNode>(
     "CostmapDownsamplerTest");
-  smac_planner::CostmapDownsampler downsampler;
+  nav2_smac_planner::CostmapDownsampler downsampler;
 
   // create basic costmap
   nav2_costmap_2d::Costmap2D costmapA(10, 10, 0.05, 0.0, 0.0, 0);
diff --git a/smac_planner/test/test_node2d.cpp b/nav2_smac_planner/test/test_node2d.cpp
similarity index 52%
rename from smac_planner/test/test_node2d.cpp
rename to nav2_smac_planner/test/test_node2d.cpp
index bc42988a..33377254 100644
--- a/smac_planner/test/test_node2d.cpp
+++ b/nav2_smac_planner/test/test_node2d.cpp
@@ -21,8 +21,8 @@
 #include "nav2_costmap_2d/costmap_2d.hpp"
 #include "nav2_costmap_2d/costmap_subscriber.hpp"
 #include "nav2_util/lifecycle_node.hpp"
-#include "smac_planner/node_2d.hpp"
-#include "smac_planner/collision_checker.hpp"
+#include "nav2_smac_planner/node_2d.hpp"
+#include "nav2_smac_planner/collision_checker.hpp"
 
 class RclCppFixture
 {
@@ -35,12 +35,12 @@ RclCppFixture g_rclcppfixture;
 TEST(Node2DTest, test_node_2d)
 {
   nav2_costmap_2d::Costmap2D costmapA(10, 10, 0.05, 0.0, 0.0, 0);
-  smac_planner::GridCollisionChecker checker(&costmapA);
+  nav2_smac_planner::GridCollisionChecker checker(&costmapA);
 
   // test construction
   unsigned char cost = static_cast<unsigned char>(1);
-  smac_planner::Node2D testA(cost, 1);
-  smac_planner::Node2D testB(cost, 1);
+  nav2_smac_planner::Node2D testA(cost, 1);
+  nav2_smac_planner::Node2D testB(cost, 1);
   EXPECT_EQ(testA.getCost(), 1.0f);
 
   // test reset
@@ -63,13 +63,13 @@ TEST(Node2DTest, test_node_2d)
   EXPECT_EQ(testB.getTraversalCost(&testA), 58.0f);
 
   // check heuristic cost computation
-  smac_planner::Node2D::Coordinates A(0.0, 0.0);
-  smac_planner::Node2D::Coordinates B(10.0, 5.0);
+  nav2_smac_planner::Node2D::Coordinates A(0.0, 0.0);
+  nav2_smac_planner::Node2D::Coordinates B(10.0, 5.0);
   EXPECT_NEAR(testB.getHeuristicCost(A, B), 559.016, 0.01);
 
   // check operator== works on index
   unsigned char costC = '2';
-  smac_planner::Node2D testC(costC, 1);
+  nav2_smac_planner::Node2D testC(costC, 1);
   EXPECT_TRUE(testA == testC);
 
   // check accumulated costs are set
@@ -88,46 +88,46 @@ TEST(Node2DTest, test_node_2d)
   EXPECT_EQ(testC.getIndex(), 1u);
 
   // check static index functions
-  EXPECT_EQ(smac_planner::Node2D::getIndex(1u, 1u, 10u), 11u);
-  EXPECT_EQ(smac_planner::Node2D::getIndex(6u, 43u, 10u), 436u);
-  EXPECT_EQ(smac_planner::Node2D::getCoords(436u, 10u, 1u).x, 6u);
-  EXPECT_EQ(smac_planner::Node2D::getCoords(436u, 10u, 1u).y, 43u);
-  EXPECT_THROW(smac_planner::Node2D::getCoords(436u, 10u, 10u), std::runtime_error);
+  EXPECT_EQ(nav2_smac_planner::Node2D::getIndex(1u, 1u, 10u), 11u);
+  EXPECT_EQ(nav2_smac_planner::Node2D::getIndex(6u, 43u, 10u), 436u);
+  EXPECT_EQ(nav2_smac_planner::Node2D::getCoords(436u, 10u, 1u).x, 6u);
+  EXPECT_EQ(nav2_smac_planner::Node2D::getCoords(436u, 10u, 1u).y, 43u);
+  EXPECT_THROW(nav2_smac_planner::Node2D::getCoords(436u, 10u, 10u), std::runtime_error);
 }
 
 TEST(Node2DTest, test_node_2d_neighbors)
 {
   // test neighborhood computation
-  smac_planner::Node2D::initNeighborhood(10u, smac_planner::MotionModel::VON_NEUMANN);
-  EXPECT_EQ(smac_planner::Node2D::_neighbors_grid_offsets.size(), 4u);
-  EXPECT_EQ(smac_planner::Node2D::_neighbors_grid_offsets[0], -1);
-  EXPECT_EQ(smac_planner::Node2D::_neighbors_grid_offsets[1], 1);
-  EXPECT_EQ(smac_planner::Node2D::_neighbors_grid_offsets[2], -10);
-  EXPECT_EQ(smac_planner::Node2D::_neighbors_grid_offsets[3], 10);
-
-  smac_planner::Node2D::initNeighborhood(100u, smac_planner::MotionModel::MOORE);
-  EXPECT_EQ(smac_planner::Node2D::_neighbors_grid_offsets.size(), 8u);
-  EXPECT_EQ(smac_planner::Node2D::_neighbors_grid_offsets[0], -1);
-  EXPECT_EQ(smac_planner::Node2D::_neighbors_grid_offsets[1], 1);
-  EXPECT_EQ(smac_planner::Node2D::_neighbors_grid_offsets[2], -100);
-  EXPECT_EQ(smac_planner::Node2D::_neighbors_grid_offsets[3], 100);
-  EXPECT_EQ(smac_planner::Node2D::_neighbors_grid_offsets[4], -101);
-  EXPECT_EQ(smac_planner::Node2D::_neighbors_grid_offsets[5], -99);
-  EXPECT_EQ(smac_planner::Node2D::_neighbors_grid_offsets[6], 99);
-  EXPECT_EQ(smac_planner::Node2D::_neighbors_grid_offsets[7], 101);
+  nav2_smac_planner::Node2D::initNeighborhood(10u, nav2_smac_planner::MotionModel::VON_NEUMANN);
+  EXPECT_EQ(nav2_smac_planner::Node2D::_neighbors_grid_offsets.size(), 4u);
+  EXPECT_EQ(nav2_smac_planner::Node2D::_neighbors_grid_offsets[0], -1);
+  EXPECT_EQ(nav2_smac_planner::Node2D::_neighbors_grid_offsets[1], 1);
+  EXPECT_EQ(nav2_smac_planner::Node2D::_neighbors_grid_offsets[2], -10);
+  EXPECT_EQ(nav2_smac_planner::Node2D::_neighbors_grid_offsets[3], 10);
+
+  nav2_smac_planner::Node2D::initNeighborhood(100u, nav2_smac_planner::MotionModel::MOORE);
+  EXPECT_EQ(nav2_smac_planner::Node2D::_neighbors_grid_offsets.size(), 8u);
+  EXPECT_EQ(nav2_smac_planner::Node2D::_neighbors_grid_offsets[0], -1);
+  EXPECT_EQ(nav2_smac_planner::Node2D::_neighbors_grid_offsets[1], 1);
+  EXPECT_EQ(nav2_smac_planner::Node2D::_neighbors_grid_offsets[2], -100);
+  EXPECT_EQ(nav2_smac_planner::Node2D::_neighbors_grid_offsets[3], 100);
+  EXPECT_EQ(nav2_smac_planner::Node2D::_neighbors_grid_offsets[4], -101);
+  EXPECT_EQ(nav2_smac_planner::Node2D::_neighbors_grid_offsets[5], -99);
+  EXPECT_EQ(nav2_smac_planner::Node2D::_neighbors_grid_offsets[6], 99);
+  EXPECT_EQ(nav2_smac_planner::Node2D::_neighbors_grid_offsets[7], 101);
 
   nav2_costmap_2d::Costmap2D costmapA(10, 10, 0.05, 0.0, 0.0, 0);
-  smac_planner::GridCollisionChecker checker(&costmapA);
+  nav2_smac_planner::GridCollisionChecker checker(&costmapA);
   unsigned char cost = static_cast<unsigned int>(1);
-  smac_planner::Node2D * node = new smac_planner::Node2D(cost, 1);
-  std::function<bool(const unsigned int &, smac_planner::Node2D * &)> neighborGetter =
-    [&, this](const unsigned int & index, smac_planner::Node2D * & neighbor_rtn) -> bool
+  nav2_smac_planner::Node2D * node = new nav2_smac_planner::Node2D(cost, 1);
+  std::function<bool(const unsigned int &, nav2_smac_planner::Node2D * &)> neighborGetter =
+    [&, this](const unsigned int & index, nav2_smac_planner::Node2D * & neighbor_rtn) -> bool
     {
       return true;
     };
 
-  smac_planner::Node2D::NodeVector neighbors;
-  smac_planner::Node2D::getNeighbors(node, neighborGetter, checker, false, neighbors);
+  nav2_smac_planner::Node2D::NodeVector neighbors;
+  nav2_smac_planner::Node2D::getNeighbors(node, neighborGetter, checker, false, neighbors);
   delete node;
 
   // should be empty since totally invalid
diff --git a/smac_planner/test/test_nodebasic.cpp b/nav2_smac_planner/test/test_nodebasic.cpp
similarity index 79%
rename from smac_planner/test/test_nodebasic.cpp
rename to nav2_smac_planner/test/test_nodebasic.cpp
index 0ec90a11..23d445ce 100644
--- a/smac_planner/test/test_nodebasic.cpp
+++ b/nav2_smac_planner/test/test_nodebasic.cpp
@@ -22,10 +22,10 @@
 #include "nav2_costmap_2d/costmap_2d.hpp"
 #include "nav2_costmap_2d/costmap_subscriber.hpp"
 #include "nav2_util/lifecycle_node.hpp"
-#include "smac_planner/node_basic.hpp"
-#include "smac_planner/node_2d.hpp"
-#include "smac_planner/node_se2.hpp"
-#include "smac_planner/collision_checker.hpp"
+#include "nav2_smac_planner/node_basic.hpp"
+#include "nav2_smac_planner/node_2d.hpp"
+#include "nav2_smac_planner/node_se2.hpp"
+#include "nav2_smac_planner/collision_checker.hpp"
 
 class RclCppFixture
 {
@@ -37,12 +37,12 @@ RclCppFixture g_rclcppfixture;
 
 TEST(NodeBasicTest, test_node_basic)
 {
-  smac_planner::NodeBasic<smac_planner::NodeSE2> node(50);
+  nav2_smac_planner::NodeBasic<nav2_smac_planner::NodeSE2> node(50);
 
   EXPECT_EQ(node.index, 50u);
   EXPECT_EQ(node.graph_node_ptr, nullptr);
 
-  smac_planner::NodeBasic<smac_planner::Node2D> node2(100);
+  nav2_smac_planner::NodeBasic<nav2_smac_planner::Node2D> node2(100);
 
   EXPECT_EQ(node2.index, 100u);
   EXPECT_EQ(node2.graph_node_ptr, nullptr);
diff --git a/nav2_smac_planner/test/test_nodese2.cpp b/nav2_smac_planner/test/test_nodese2.cpp
new file mode 100644
index 00000000..39b9aa2d
--- /dev/null
+++ b/nav2_smac_planner/test/test_nodese2.cpp
@@ -0,0 +1,214 @@
+// Copyright (c) 2020, 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.
+
+#include <math.h>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "gtest/gtest.h"
+#include "rclcpp/rclcpp.hpp"
+#include "nav2_costmap_2d/costmap_2d.hpp"
+#include "nav2_costmap_2d/costmap_subscriber.hpp"
+#include "nav2_util/lifecycle_node.hpp"
+#include "nav2_smac_planner/node_se2.hpp"
+#include "nav2_smac_planner/collision_checker.hpp"
+
+class RclCppFixture
+{
+public:
+  RclCppFixture() {rclcpp::init(0, nullptr);}
+  ~RclCppFixture() {rclcpp::shutdown();}
+};
+RclCppFixture g_rclcppfixture;
+
+TEST(NodeSE2Test, test_node_se2)
+{
+  nav2_smac_planner::SearchInfo info;
+  info.change_penalty = 1.2;
+  info.non_straight_penalty = 1.4;
+  info.reverse_penalty = 2.1;
+  info.minimum_turning_radius = 0.20;
+  unsigned int size_x = 10;
+  unsigned int size_y = 10;
+  unsigned int size_theta = 72;
+
+  nav2_smac_planner::NodeSE2::initMotionModel(
+    nav2_smac_planner::MotionModel::DUBIN, size_x, size_y, size_theta, info);
+
+  nav2_costmap_2d::Costmap2D * costmapA = new nav2_costmap_2d::Costmap2D(
+    10, 10, 0.05, 0.0, 0.0, 0);
+  nav2_smac_planner::GridCollisionChecker checker(costmapA);
+  checker.setFootprint(nav2_costmap_2d::Footprint(), true);
+
+  // test construction
+  nav2_smac_planner::NodeSE2 testA(49);
+  nav2_smac_planner::NodeSE2 testB(49);
+  EXPECT_TRUE(std::isnan(testA.getCost()));
+
+  // test node valid and cost
+  testA.pose.x = 5;
+  testA.pose.y = 5;
+  testA.pose.theta = 0;
+  EXPECT_EQ(testA.isNodeValid(true, checker), true);
+  EXPECT_EQ(testA.isNodeValid(false, checker), true);
+  EXPECT_EQ(testA.getCost(), 0.0f);
+
+  // test reset
+  testA.reset();
+  EXPECT_TRUE(std::isnan(testA.getCost()));
+
+  // Check constants
+  EXPECT_EQ(testA.neutral_cost, sqrt(2));
+
+  // check collision checking
+  EXPECT_EQ(testA.isNodeValid(false, checker), true);
+
+  // check traversal cost computation
+  // simulated first node, should return neutral cost
+  EXPECT_NEAR(testB.getTraversalCost(&testA), sqrt(2), 0.01);
+  // now with straight motion, cost is 0, so will be sqrt(2) as well
+  testB.setMotionPrimitiveIndex(1);
+  testA.setMotionPrimitiveIndex(0);
+  EXPECT_NEAR(testB.getTraversalCost(&testA), sqrt(2), 0.01);
+  // same direction as parent, testB
+  testA.setMotionPrimitiveIndex(1);
+  EXPECT_NEAR(testB.getTraversalCost(&testA), 1.9799f, 0.01);
+  // opposite direction as parent, testB
+  testA.setMotionPrimitiveIndex(2);
+  EXPECT_NEAR(testB.getTraversalCost(&testA), 3.67696f, 0.01);
+
+  // will throw because never collision checked testB
+  EXPECT_THROW(testA.getTraversalCost(&testB), std::runtime_error);
+
+  // check motion primitives
+  EXPECT_EQ(testA.getMotionPrimitiveIndex(), 2u);
+
+  // check heuristic cost computation
+  nav2_smac_planner::NodeSE2::computeWavefrontHeuristic(
+    costmapA,
+    static_cast<unsigned int>(10.0),
+    static_cast<unsigned int>(5.0),
+    0.0, 0.0);
+  nav2_smac_planner::NodeSE2::Coordinates A(0.0, 0.0, 4.2);
+  nav2_smac_planner::NodeSE2::Coordinates B(10.0, 5.0, 54.1);
+  EXPECT_NEAR(testB.getHeuristicCost(B, A), 16.723, 0.01);
+
+  // check operator== works on index
+  nav2_smac_planner::NodeSE2 testC(49);
+  EXPECT_TRUE(testA == testC);
+
+  // check accumulated costs are set
+  testC.setAccumulatedCost(100);
+  EXPECT_EQ(testC.getAccumulatedCost(), 100.0f);
+
+  // check visiting state
+  EXPECT_EQ(testC.wasVisited(), false);
+  testC.queued();
+  EXPECT_EQ(testC.isQueued(), true);
+  testC.visited();
+  EXPECT_EQ(testC.wasVisited(), true);
+  EXPECT_EQ(testC.isQueued(), false);
+
+  // check index
+  EXPECT_EQ(testC.getIndex(), 49u);
+
+  // check set pose and pose
+  testC.setPose(nav2_smac_planner::NodeSE2::Coordinates(10.0, 5.0, 4));
+  EXPECT_EQ(testC.pose.x, 10.0);
+  EXPECT_EQ(testC.pose.y, 5.0);
+  EXPECT_EQ(testC.pose.theta, 4);
+
+  // check static index functions
+  EXPECT_EQ(nav2_smac_planner::NodeSE2::getIndex(1u, 1u, 4u, 10u, 72u), 796u);
+  EXPECT_EQ(nav2_smac_planner::NodeSE2::getCoords(796u, 10u, 72u).x, 1u);
+  EXPECT_EQ(nav2_smac_planner::NodeSE2::getCoords(796u, 10u, 72u).y, 1u);
+  EXPECT_EQ(nav2_smac_planner::NodeSE2::getCoords(796u, 10u, 72u).theta, 4u);
+
+  delete costmapA;
+}
+
+TEST(NodeSE2Test, test_node_2d_neighbors)
+{
+  nav2_smac_planner::SearchInfo info;
+  info.change_penalty = 1.2;
+  info.non_straight_penalty = 1.4;
+  info.reverse_penalty = 2.1;
+  info.minimum_turning_radius = 4;  // 0.2 in grid coordinates
+  unsigned int size_x = 100;
+  unsigned int size_y = 100;
+  unsigned int size_theta = 72;
+  nav2_smac_planner::NodeSE2::initMotionModel(
+    nav2_smac_planner::MotionModel::DUBIN, size_x, size_y, size_theta, info);
+
+
+  // test neighborhood computation
+  EXPECT_EQ(nav2_smac_planner::NodeSE2::motion_table.projections.size(), 3u);
+  EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[0]._x, 1.731517, 0.01);
+  EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[0]._y, 0, 0.01);
+  EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[0]._theta, 0, 0.01);
+
+  EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[1]._x, 1.69047, 0.01);
+  EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[1]._y, 0.3747, 0.01);
+  EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[1]._theta, 5, 0.01);
+
+  EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[2]._x, 1.69047, 0.01);
+  EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[2]._y, -0.3747, 0.01);
+  EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[2]._theta, -5, 0.01);
+
+  nav2_smac_planner::NodeSE2::initMotionModel(
+    nav2_smac_planner::MotionModel::REEDS_SHEPP, size_x, size_y, size_theta, info);
+
+  EXPECT_EQ(nav2_smac_planner::NodeSE2::motion_table.projections.size(), 6u);
+  EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[0]._x, 1.731517, 0.01);
+  EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[0]._y, 0, 0.01);
+  EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[0]._theta, 0, 0.01);
+
+  EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[1]._x, 1.69047, 0.01);
+  EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[1]._y, 0.3747, 0.01);
+  EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[1]._theta, 5, 0.01);
+
+  EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[2]._x, 1.69047, 0.01);
+  EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[2]._y, -0.3747, 0.01);
+  EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[2]._theta, -5, 0.01);
+
+  EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[3]._x, -1.731517, 0.01);
+  EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[3]._y, 0, 0.01);
+  EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[3]._theta, 0, 0.01);
+
+  EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[4]._x, -1.69047, 0.01);
+  EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[4]._y, 0.3747, 0.01);
+  EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[4]._theta, -5, 0.01);
+
+  EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[5]._x, -1.69047, 0.01);
+  EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[5]._y, -0.3747, 0.01);
+  EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[5]._theta, 5, 0.01);
+
+  nav2_costmap_2d::Costmap2D costmapA(100, 100, 0.05, 0.0, 0.0, 0);
+  nav2_smac_planner::GridCollisionChecker checker(&costmapA);
+  nav2_smac_planner::NodeSE2 * node = new nav2_smac_planner::NodeSE2(49);
+  std::function<bool(const unsigned int &, nav2_smac_planner::NodeSE2 * &)> neighborGetter =
+    [&, this](const unsigned int & index, nav2_smac_planner::NodeSE2 * & neighbor_rtn) -> bool
+    {
+      // because we don't return a real object
+      return false;
+    };
+
+  nav2_smac_planner::NodeSE2::NodeVector neighbors;
+  nav2_smac_planner::NodeSE2::getNeighbors(node, neighborGetter, checker, false, neighbors);
+  delete node;
+
+  // should be empty since totally invalid
+  EXPECT_EQ(neighbors.size(), 0u);
+}
diff --git a/smac_planner/test/test_smac_2d.cpp b/nav2_smac_planner/test/test_smac_2d.cpp
similarity index 89%
rename from smac_planner/test/test_smac_2d.cpp
rename to nav2_smac_planner/test/test_smac_2d.cpp
index 63b4ba99..59a31eb1 100644
--- a/smac_planner/test/test_smac_2d.cpp
+++ b/nav2_smac_planner/test/test_smac_2d.cpp
@@ -23,11 +23,11 @@
 #include "nav2_costmap_2d/costmap_subscriber.hpp"
 #include "nav2_util/lifecycle_node.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
-#include "smac_planner/node_se2.hpp"
-#include "smac_planner/a_star.hpp"
-#include "smac_planner/collision_checker.hpp"
-#include "smac_planner/smac_planner.hpp"
-#include "smac_planner/smac_planner_2d.hpp"
+#include "nav2_smac_planner/node_se2.hpp"
+#include "nav2_smac_planner/a_star.hpp"
+#include "nav2_smac_planner/collision_checker.hpp"
+#include "nav2_smac_planner/smac_planner.hpp"
+#include "nav2_smac_planner/smac_planner_2d.hpp"
 
 class RclCppFixture
 {
@@ -62,7 +62,7 @@ TEST(SmacTest, test_smac_2d)
   start.pose.position.y = 0.0;
   start.pose.orientation.w = 1.0;
   goal = start;
-  auto planner_2d = std::make_unique<smac_planner::SmacPlanner2D>();
+  auto planner_2d = std::make_unique<nav2_smac_planner::SmacPlanner2D>();
   planner_2d->configure(node2D, "test", nullptr, costmap_ros);
   planner_2d->activate();
   try {
diff --git a/smac_planner/test/test_smac_se2.cpp b/nav2_smac_planner/test/test_smac_se2.cpp
similarity index 87%
rename from smac_planner/test/test_smac_se2.cpp
rename to nav2_smac_planner/test/test_smac_se2.cpp
index caf7d338..c0a4cf8e 100644
--- a/smac_planner/test/test_smac_se2.cpp
+++ b/nav2_smac_planner/test/test_smac_se2.cpp
@@ -23,11 +23,11 @@
 #include "nav2_costmap_2d/costmap_subscriber.hpp"
 #include "nav2_util/lifecycle_node.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
-#include "smac_planner/node_se2.hpp"
-#include "smac_planner/a_star.hpp"
-#include "smac_planner/collision_checker.hpp"
-#include "smac_planner/smac_planner.hpp"
-#include "smac_planner/smac_planner_2d.hpp"
+#include "nav2_smac_planner/node_se2.hpp"
+#include "nav2_smac_planner/a_star.hpp"
+#include "nav2_smac_planner/collision_checker.hpp"
+#include "nav2_smac_planner/smac_planner.hpp"
+#include "nav2_smac_planner/smac_planner_2d.hpp"
 
 class RclCppFixture
 {
@@ -62,7 +62,7 @@ TEST(SmacTest, test_smac_se2)
   start.pose.position.y = 0.0;
   start.pose.orientation.w = 1.0;
   goal = start;
-  auto planner = std::make_unique<smac_planner::SmacPlanner>();
+  auto planner = std::make_unique<nav2_smac_planner::SmacPlanner>();
   planner->configure(nodeSE2, "test", nullptr, costmap_ros);
   planner->activate();
 
@@ -88,5 +88,5 @@ TEST(SmacTestSE2, test_dist)
   Eigen::Vector2d p2;
   p2[0] = 0.0;
   p2[1] = 1.0;
-  EXPECT_NEAR(smac_planner::squaredDistance(p1, p2), 1.0, 0.001);
+  EXPECT_NEAR(nav2_smac_planner::squaredDistance(p1, p2), 1.0, 0.001);
 }
diff --git a/smac_planner/test/test_smoother.cpp b/nav2_smac_planner/test/test_smoother.cpp
similarity index 87%
rename from smac_planner/test/test_smoother.cpp
rename to nav2_smac_planner/test/test_smoother.cpp
index ab05a939..41c82809 100644
--- a/smac_planner/test/test_smoother.cpp
+++ b/nav2_smac_planner/test/test_smoother.cpp
@@ -21,8 +21,8 @@
 #include "rclcpp/rclcpp.hpp"
 #include "nav2_costmap_2d/costmap_2d.hpp"
 #include "nav2_util/lifecycle_node.hpp"
-#include "smac_planner/a_star.hpp"
-#include "smac_planner/smoother.hpp"
+#include "nav2_smac_planner/a_star.hpp"
+#include "nav2_smac_planner/smoother.hpp"
 
 class RclCppFixture
 {
@@ -47,14 +47,14 @@ TEST(SmootherTest, test_smoother)
   }
 
   // compute path to use
-  smac_planner::SearchInfo info;
+  nav2_smac_planner::SearchInfo info;
   info.change_penalty = 1.2;
   info.non_straight_penalty = 1.4;
   info.reverse_penalty = 2.1;
   info.minimum_turning_radius = 4.0;  // in grid coordinates
   unsigned int size_theta = 72;
-  smac_planner::AStarAlgorithm<smac_planner::NodeSE2> a_star(
-    smac_planner::MotionModel::DUBIN, info);
+  nav2_smac_planner::AStarAlgorithm<nav2_smac_planner::NodeSE2> a_star(
+    nav2_smac_planner::MotionModel::DUBIN, info);
   int max_iterations = 1000000;
   float tolerance = 0.0;
   int it_on_approach = 1000000000;
@@ -64,7 +64,7 @@ TEST(SmootherTest, test_smoother)
   a_star.createGraph(costmap->getSizeInCellsX(), costmap->getSizeInCellsY(), size_theta, costmap);
   a_star.setStart(10u, 10u, 0u);
   a_star.setGoal(80u, 80u, 40u);
-  smac_planner::NodeSE2::CoordinateVector plan;
+  nav2_smac_planner::NodeSE2::CoordinateVector plan;
   EXPECT_TRUE(a_star.createPath(plan, num_it, tolerance));
 
   // populate our smoothing paths
@@ -75,11 +75,11 @@ TEST(SmootherTest, test_smoother)
     initial_path.push_back(Eigen::Vector2d(plan[i].x, plan[i].y));
   }
 
-  smac_planner::OptimizerParams params;
+  nav2_smac_planner::OptimizerParams params;
   params.debug = true;
   params.get(node2D.get(), "test");
 
-  smac_planner::SmootherParams smoother_params;
+  nav2_smac_planner::SmootherParams smoother_params;
   smoother_params.get(node2D.get(), "test");
   smoother_params.max_curvature = 5.0;
   smoother_params.curvature_weight = 30.0;
@@ -87,7 +87,7 @@ TEST(SmootherTest, test_smoother)
   smoother_params.smooth_weight = 00.0;
   smoother_params.costmap_weight = 0.025;
 
-  smac_planner::Smoother smoother;
+  nav2_smac_planner::Smoother smoother;
   smoother.initialize(params);
   smoother.smooth(path, costmap, smoother_params);
 
diff --git a/navigation2/package.xml b/navigation2/package.xml
index bd72a39f..989ce6aa 100644
--- a/navigation2/package.xml
+++ b/navigation2/package.xml
@@ -30,7 +30,7 @@
   <exec_depend>nav2_voxel_grid</exec_depend>
   <exec_depend>nav2_controller</exec_depend>
   <exec_depend>nav2_waypoint_follower</exec_depend>
-  <exec_depend>smac_planner</exec_depend>
+  <exec_depend>nav2_smac_planner</exec_depend>
 
   <export>
     <build_type>ament_cmake</build_type>
diff --git a/smac_planner/smac_plugin.xml b/smac_planner/smac_plugin.xml
deleted file mode 100644
index 09f17b66..00000000
--- a/smac_planner/smac_plugin.xml
+++ /dev/null
@@ -1,5 +0,0 @@
-<library path="smac_planner">
-	<class name="smac_planner/SmacPlanner" type="smac_planner::SmacPlanner" base_class_type="nav2_core::GlobalPlanner">
-	  <description>SE2 version of the SMAC planner</description>
-	</class>
-</library>
diff --git a/smac_planner/smac_plugin_2d.xml b/smac_planner/smac_plugin_2d.xml
deleted file mode 100644
index 3845a0ff..00000000
--- a/smac_planner/smac_plugin_2d.xml
+++ /dev/null
@@ -1,5 +0,0 @@
-<library path="smac_planner_2d">
-  <class name="smac_planner/SmacPlanner2D" type="smac_planner::SmacPlanner2D" base_class_type="nav2_core::GlobalPlanner">
-    <description>2D version of the SMAC Planner</description>
-  </class>
-</library>
diff --git a/smac_planner/test/test_nodese2.cpp b/smac_planner/test/test_nodese2.cpp
deleted file mode 100644
index 2f23c12a..00000000
--- a/smac_planner/test/test_nodese2.cpp
+++ /dev/null
@@ -1,214 +0,0 @@
-// Copyright (c) 2020, 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.
-
-#include <math.h>
-#include <memory>
-#include <string>
-#include <vector>
-
-#include "gtest/gtest.h"
-#include "rclcpp/rclcpp.hpp"
-#include "nav2_costmap_2d/costmap_2d.hpp"
-#include "nav2_costmap_2d/costmap_subscriber.hpp"
-#include "nav2_util/lifecycle_node.hpp"
-#include "smac_planner/node_se2.hpp"
-#include "smac_planner/collision_checker.hpp"
-
-class RclCppFixture
-{
-public:
-  RclCppFixture() {rclcpp::init(0, nullptr);}
-  ~RclCppFixture() {rclcpp::shutdown();}
-};
-RclCppFixture g_rclcppfixture;
-
-TEST(NodeSE2Test, test_node_se2)
-{
-  smac_planner::SearchInfo info;
-  info.change_penalty = 1.2;
-  info.non_straight_penalty = 1.4;
-  info.reverse_penalty = 2.1;
-  info.minimum_turning_radius = 0.20;
-  unsigned int size_x = 10;
-  unsigned int size_y = 10;
-  unsigned int size_theta = 72;
-
-  smac_planner::NodeSE2::initMotionModel(
-    smac_planner::MotionModel::DUBIN, size_x, size_y, size_theta, info);
-
-  nav2_costmap_2d::Costmap2D * costmapA = new nav2_costmap_2d::Costmap2D(
-    10, 10, 0.05, 0.0, 0.0, 0);
-  smac_planner::GridCollisionChecker checker(costmapA);
-  checker.setFootprint(nav2_costmap_2d::Footprint(), true);
-
-  // test construction
-  smac_planner::NodeSE2 testA(49);
-  smac_planner::NodeSE2 testB(49);
-  EXPECT_TRUE(std::isnan(testA.getCost()));
-
-  // test node valid and cost
-  testA.pose.x = 5;
-  testA.pose.y = 5;
-  testA.pose.theta = 0;
-  EXPECT_EQ(testA.isNodeValid(true, checker), true);
-  EXPECT_EQ(testA.isNodeValid(false, checker), true);
-  EXPECT_EQ(testA.getCost(), 0.0f);
-
-  // test reset
-  testA.reset();
-  EXPECT_TRUE(std::isnan(testA.getCost()));
-
-  // Check constants
-  EXPECT_EQ(testA.neutral_cost, sqrt(2));
-
-  // check collision checking
-  EXPECT_EQ(testA.isNodeValid(false, checker), true);
-
-  // check traversal cost computation
-  // simulated first node, should return neutral cost
-  EXPECT_NEAR(testB.getTraversalCost(&testA), sqrt(2), 0.01);
-  // now with straight motion, cost is 0, so will be sqrt(2) as well
-  testB.setMotionPrimitiveIndex(1);
-  testA.setMotionPrimitiveIndex(0);
-  EXPECT_NEAR(testB.getTraversalCost(&testA), sqrt(2), 0.01);
-  // same direction as parent, testB
-  testA.setMotionPrimitiveIndex(1);
-  EXPECT_NEAR(testB.getTraversalCost(&testA), 1.9799f, 0.01);
-  // opposite direction as parent, testB
-  testA.setMotionPrimitiveIndex(2);
-  EXPECT_NEAR(testB.getTraversalCost(&testA), 3.67696f, 0.01);
-
-  // will throw because never collision checked testB
-  EXPECT_THROW(testA.getTraversalCost(&testB), std::runtime_error);
-
-  // check motion primitives
-  EXPECT_EQ(testA.getMotionPrimitiveIndex(), 2u);
-
-  // check heuristic cost computation
-  smac_planner::NodeSE2::computeWavefrontHeuristic(
-    costmapA,
-    static_cast<unsigned int>(10.0),
-    static_cast<unsigned int>(5.0),
-    0.0, 0.0);
-  smac_planner::NodeSE2::Coordinates A(0.0, 0.0, 4.2);
-  smac_planner::NodeSE2::Coordinates B(10.0, 5.0, 54.1);
-  EXPECT_NEAR(testB.getHeuristicCost(B, A), 16.723, 0.01);
-
-  // check operator== works on index
-  smac_planner::NodeSE2 testC(49);
-  EXPECT_TRUE(testA == testC);
-
-  // check accumulated costs are set
-  testC.setAccumulatedCost(100);
-  EXPECT_EQ(testC.getAccumulatedCost(), 100.0f);
-
-  // check visiting state
-  EXPECT_EQ(testC.wasVisited(), false);
-  testC.queued();
-  EXPECT_EQ(testC.isQueued(), true);
-  testC.visited();
-  EXPECT_EQ(testC.wasVisited(), true);
-  EXPECT_EQ(testC.isQueued(), false);
-
-  // check index
-  EXPECT_EQ(testC.getIndex(), 49u);
-
-  // check set pose and pose
-  testC.setPose(smac_planner::NodeSE2::Coordinates(10.0, 5.0, 4));
-  EXPECT_EQ(testC.pose.x, 10.0);
-  EXPECT_EQ(testC.pose.y, 5.0);
-  EXPECT_EQ(testC.pose.theta, 4);
-
-  // check static index functions
-  EXPECT_EQ(smac_planner::NodeSE2::getIndex(1u, 1u, 4u, 10u, 72u), 796u);
-  EXPECT_EQ(smac_planner::NodeSE2::getCoords(796u, 10u, 72u).x, 1u);
-  EXPECT_EQ(smac_planner::NodeSE2::getCoords(796u, 10u, 72u).y, 1u);
-  EXPECT_EQ(smac_planner::NodeSE2::getCoords(796u, 10u, 72u).theta, 4u);
-
-  delete costmapA;
-}
-
-TEST(NodeSE2Test, test_node_2d_neighbors)
-{
-  smac_planner::SearchInfo info;
-  info.change_penalty = 1.2;
-  info.non_straight_penalty = 1.4;
-  info.reverse_penalty = 2.1;
-  info.minimum_turning_radius = 4;  // 0.2 in grid coordinates
-  unsigned int size_x = 100;
-  unsigned int size_y = 100;
-  unsigned int size_theta = 72;
-  smac_planner::NodeSE2::initMotionModel(
-    smac_planner::MotionModel::DUBIN, size_x, size_y, size_theta, info);
-
-
-  // test neighborhood computation
-  EXPECT_EQ(smac_planner::NodeSE2::motion_table.projections.size(), 3u);
-  EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[0]._x, 1.731517, 0.01);
-  EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[0]._y, 0, 0.01);
-  EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[0]._theta, 0, 0.01);
-
-  EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[1]._x, 1.69047, 0.01);
-  EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[1]._y, 0.3747, 0.01);
-  EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[1]._theta, 5, 0.01);
-
-  EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[2]._x, 1.69047, 0.01);
-  EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[2]._y, -0.3747, 0.01);
-  EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[2]._theta, -5, 0.01);
-
-  smac_planner::NodeSE2::initMotionModel(
-    smac_planner::MotionModel::REEDS_SHEPP, size_x, size_y, size_theta, info);
-
-  EXPECT_EQ(smac_planner::NodeSE2::motion_table.projections.size(), 6u);
-  EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[0]._x, 1.731517, 0.01);
-  EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[0]._y, 0, 0.01);
-  EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[0]._theta, 0, 0.01);
-
-  EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[1]._x, 1.69047, 0.01);
-  EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[1]._y, 0.3747, 0.01);
-  EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[1]._theta, 5, 0.01);
-
-  EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[2]._x, 1.69047, 0.01);
-  EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[2]._y, -0.3747, 0.01);
-  EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[2]._theta, -5, 0.01);
-
-  EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[3]._x, -1.731517, 0.01);
-  EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[3]._y, 0, 0.01);
-  EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[3]._theta, 0, 0.01);
-
-  EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[4]._x, -1.69047, 0.01);
-  EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[4]._y, 0.3747, 0.01);
-  EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[4]._theta, -5, 0.01);
-
-  EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[5]._x, -1.69047, 0.01);
-  EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[5]._y, -0.3747, 0.01);
-  EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[5]._theta, 5, 0.01);
-
-  nav2_costmap_2d::Costmap2D costmapA(100, 100, 0.05, 0.0, 0.0, 0);
-  smac_planner::GridCollisionChecker checker(&costmapA);
-  smac_planner::NodeSE2 * node = new smac_planner::NodeSE2(49);
-  std::function<bool(const unsigned int &, smac_planner::NodeSE2 * &)> neighborGetter =
-    [&, this](const unsigned int & index, smac_planner::NodeSE2 * & neighbor_rtn) -> bool
-    {
-      // because we don't return a real object
-      return false;
-    };
-
-  smac_planner::NodeSE2::NodeVector neighbors;
-  smac_planner::NodeSE2::getNeighbors(node, neighborGetter, checker, false, neighbors);
-  delete node;
-
-  // should be empty since totally invalid
-  EXPECT_EQ(neighbors.size(), 0u);
-}
-- 
GitLab