diff --git a/Controllers/Packages/amr_control/CMakeLists.txt b/Controllers/Packages/amr_control/CMakeLists.txt
index 509a440..e89ffa8 100644
--- a/Controllers/Packages/amr_control/CMakeLists.txt
+++ b/Controllers/Packages/amr_control/CMakeLists.txt
@@ -44,6 +44,7 @@ catkin_package(
     geometry_msgs  
     loc_core 
     nav_2d_utils 
+    robot_nav_2d_utils
     move_base_core 
     robot_cpp
     roscpp 
@@ -106,12 +107,12 @@ add_dependencies(${PROJECT_NAME}_node
   ${${PROJECT_NAME}_EXPORTED_TARGETS} 
   ${catkin_EXPORTED_TARGETS}
 )
-# Fix circular RPATH dependency: disable automatic RPATH detection to avoid cycle warnings
-# Libraries will be found via LD_LIBRARY_PATH or system paths
+# Configure RPATH to find libraries in devel space
+# Use ORIGIN to find libraries relative to executable location
 set_target_properties(${PROJECT_NAME}_node PROPERTIES
-    SKIP_BUILD_RPATH TRUE
     BUILD_WITH_INSTALL_RPATH FALSE
-    INSTALL_RPATH_USE_LINK_PATH FALSE
+    INSTALL_RPATH_USE_LINK_PATH TRUE
+    BUILD_RPATH "${CATKIN_DEVEL_PREFIX}/lib"
 )
 
 # add_dependencies(vda_5050_api_test
diff --git a/Controllers/Packages/amr_control/include/amr_control/amr_control.h b/Controllers/Packages/amr_control/include/amr_control/amr_control.h
index 81b9e93..1de21dc 100644
--- a/Controllers/Packages/amr_control/include/amr_control/amr_control.h
+++ b/Controllers/Packages/amr_control/include/amr_control/amr_control.h
@@ -74,7 +74,7 @@ namespace amr_control
     bool initalized_;
     ros::NodeHandle nh_;
     std::shared_ptr<tf2_ros::Buffer> tf_;
-    // robot::TFListenerPtr tf_core_ptr_;
+    robot::TFListenerPtr tf_core_ptr_;
 
     ros::Subscriber is_detected_maker_sub_;
 
diff --git a/Controllers/Packages/amr_control/src/amr_control.cpp b/Controllers/Packages/amr_control/src/amr_control.cpp
index 314fbb0..2639df2 100644
--- a/Controllers/Packages/amr_control/src/amr_control.cpp
+++ b/Controllers/Packages/amr_control/src/amr_control.cpp
@@ -3,6 +3,7 @@
 #include <robot/plugin_loader_helper.h>
 #include <robot/console.h>  
 #include <robot/node_handle.h>
+#include <tf3/buffer_core.h>
 
 namespace amr_control
 {
@@ -16,6 +17,7 @@ namespace amr_control
         loc_base_ptr_(nullptr),
         move_base_ptr_(nullptr),
         amr_safety_ptr_(nullptr),
+        tf_core_ptr_(nullptr),
         loc_base_loader_("loc_core", "loc_core::BaseLocalization")
   {
     this->init(nh, buffer);
@@ -31,6 +33,7 @@ namespace amr_control
         loc_base_ptr_(nullptr),
         move_base_ptr_(nullptr),
         amr_safety_ptr_(nullptr),
+        tf_core_ptr_(nullptr),
         loc_base_loader_("loc_core", "loc_core::BaseLocalization")
   {
   }
@@ -79,6 +82,8 @@ namespace amr_control
     {
       nh_ = nh;
       tf_ = buffer;
+      // Create tf3::BufferCore instance for move_base
+      tf_core_ptr_ = std::make_shared<tf3::BufferCore>();
 
       monitor_thr_ = std::make_shared<std::thread>(
           [this]()
@@ -231,7 +236,7 @@ namespace amr_control
       
       robot::log_info("[%s:%d]\n Creating MoveBase instance...", __FILE__, __LINE__);
       move_base_ptr_ = move_base_loader_();
-      // move_base_ptr_->initialize(tf_core_ptr_);
+      move_base_ptr_->initialize(tf_core_ptr_);
 
       ros::Rate r(3);
       do
diff --git a/Controllers/Packages/amr_control/src/amr_control_node.cpp b/Controllers/Packages/amr_control/src/amr_control_node.cpp
index 2fac6d7..7a07589 100644
--- a/Controllers/Packages/amr_control/src/amr_control_node.cpp
+++ b/Controllers/Packages/amr_control/src/amr_control_node.cpp
@@ -21,7 +21,7 @@ int main(int argc, char** argv)
   }
   catch (const std::exception& exc)
   {
-    std::cout << exc.what() << std::endl;
+    ROS_ERROR("Exception: %s", exc.what());
   }
   return 0;
 }
\ No newline at end of file
diff --git a/pnkx_nav_core b/pnkx_nav_core
index ca9e100..6054f72 160000
--- a/pnkx_nav_core
+++ b/pnkx_nav_core
@@ -1 +1 @@
-Subproject commit ca9e100bd91c94975480244be32e6ee0a3fd0f92
+Subproject commit 6054f72ba85e8897b02e136c682a09ad3a79f5f9