diff --git a/Controllers/Packages/amr_control/CMakeLists.txt b/Controllers/Packages/amr_control/CMakeLists.txt
index a97af0b..73e7b30 100644
--- a/Controllers/Packages/amr_control/CMakeLists.txt
+++ b/Controllers/Packages/amr_control/CMakeLists.txt
@@ -23,6 +23,7 @@ find_package(catkin REQUIRED COMPONENTS
   robot_nav_2d_utils
   move_base_core
   robot_cpp
+  robot_nav_msgs
 )
 
 ## System dependencies are found with CMake's conventions
@@ -40,7 +41,7 @@ find_package(PkgConfig)
 ## DEPENDS: system dependencies of this project that dependent projects also need
 catkin_package(
   INCLUDE_DIRS include
-  LIBRARIES amr_control 
+  LIBRARIES amr_control amr_control_converter amr_control_publisher amr_control_subscriber
   CATKIN_DEPENDS 
     geometry_msgs  
     loc_core 
@@ -66,6 +67,18 @@ include_directories(
 )
 
 ## Declare a C++ library
+add_library(${PROJECT_NAME}_converter src/tf_converter.cpp src/sensor_converter.cpp)
+add_dependencies(${PROJECT_NAME}_converter ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+target_link_libraries(${PROJECT_NAME}_converter ${catkin_LIBRARIES} ${Boost_LIBRARIES})
+
+add_library(${PROJECT_NAME}_publisher src/amr_publiser.cpp)
+add_dependencies(${PROJECT_NAME}_publisher ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+target_link_libraries(${PROJECT_NAME}_publisher ${catkin_LIBRARIES} ${Boost_LIBRARIES})
+
+add_library(${PROJECT_NAME}_subscriber src/amr_subcriber.cpp)
+add_dependencies(${PROJECT_NAME}_subscriber ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+target_link_libraries(${PROJECT_NAME}_subscriber ${catkin_LIBRARIES} ${Boost_LIBRARIES})
+
 add_library(${PROJECT_NAME}
   src/amr_control.cpp
   src/amr_monitor.cpp
@@ -75,18 +88,14 @@ add_library(${PROJECT_NAME}
   src/amr_make_plan_with_order.cpp
 )
 
-add_library(${PROJECT_NAME}_converter src/tf_converter.cpp src/sensor_converter.cpp src/amr_publiser.cpp)
-add_dependencies(${PROJECT_NAME}_converter ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-target_link_libraries(${PROJECT_NAME}_converter ${catkin_LIBRARIES} ${Boost_LIBRARIES})
-
 ## Add cmake target dependencies of the library
 ## as an example, code may need to be generated before libraries
 ## either from message generation or dynamic reconfigure
-add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_converter_EXPORTED_TARGETS} ${${PROJECT_NAME}_publisher_EXPORTED_TARGETS} ${${PROJECT_NAME}_subscriber_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
-set_target_properties(${PROJECT_NAME} PROPERTIES
+set_target_properties(${PROJECT_NAME} ${PROJECT_NAME}_converter ${PROJECT_NAME}_publisher ${PROJECT_NAME}_subscriber PROPERTIES
     SKIP_BUILD_RPATH TRUE
     BUILD_WITH_INSTALL_RPATH FALSE
     INSTALL_RPATH_USE_LINK_PATH FALSE
@@ -123,6 +132,8 @@ set_target_properties(${PROJECT_NAME}_node PROPERTIES
 ## Specify libraries to link a library or executable target against
 target_link_libraries(${PROJECT_NAME}
   ${PROJECT_NAME}_converter
+  ${PROJECT_NAME}_publisher
+  ${PROJECT_NAME}_subscriber
   ${FreeOpcUa_LIBRARIES}
   ${catkin_LIBRARIES}
   ${Boost_LIBRARIES}
@@ -130,7 +141,6 @@ target_link_libraries(${PROJECT_NAME}
 
 target_link_libraries(${PROJECT_NAME}_node
   ${PROJECT_NAME}
-  ${PROJECT_NAME}_converter
   ${catkin_LIBRARIES}
   ${Boost_LIBRARIES}
 )
@@ -158,7 +168,7 @@ install(TARGETS ${PROJECT_NAME}_node
 
 ## Mark libraries for installation
 ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
-install(TARGETS ${PROJECT_NAME} 
+install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_converter ${PROJECT_NAME}_publisher ${PROJECT_NAME}_subscriber
   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
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 3f00811..174ca60 100644
--- a/Controllers/Packages/amr_control/include/amr_control/amr_control.h
+++ b/Controllers/Packages/amr_control/include/amr_control/amr_control.h
@@ -29,6 +29,7 @@
 #include <amr_control/tf_converter.h>
 #include <amr_control/sensor_converter.h>
 #include <amr_control/amr_publiser.h>
+#include <amr_control/amr_subcriber.h>
 
 #include <boost/dll/import.hpp>
 
@@ -147,7 +148,8 @@ namespace amr_control
     robot::TFListenerPtr tf_core_ptr_;
     std::shared_ptr<amr_control::TfConverter> tf_converter_ptr_;
     std::shared_ptr<amr_control::SensorConverter> sensor_converter_ptr_;
-    std::shared_ptr<amr_control::AmrPublisher> costmap_publisher_ptr_;
+    std::shared_ptr<amr_control::AmrPublisher> amr_publisher_ptr_;
+    std::shared_ptr<amr_control::AmrSubscriber> amr_subscriber_ptr_;
 
     ros::Subscriber is_detected_maker_sub_;
     std::shared_ptr<amr_control::OpcUAServerAPI> opc_ua_server_api_ptr_;
diff --git a/Controllers/Packages/amr_control/include/amr_control/amr_publiser.h b/Controllers/Packages/amr_control/include/amr_control/amr_publiser.h
index 3d47c9e..e10bee8 100644
--- a/Controllers/Packages/amr_control/include/amr_control/amr_publiser.h
+++ b/Controllers/Packages/amr_control/include/amr_control/amr_publiser.h
@@ -2,13 +2,13 @@
 #define __AMR_PUBLISHER_H_INCLUDED_
 
 #include <ros/ros.h>
+#include <robot/robot.h>
 #include <nav_msgs/OccupancyGrid.h>
 #include <geometry_msgs/PolygonStamped.h>
 #include <robot_nav_msgs/OccupancyGrid.h>
 #include <robot_map_msgs/OccupancyGridUpdate.h>
 #include <map_msgs/OccupancyGridUpdate.h>
 #include <move_base_core/navigation.h>
-#include <robot/time.h>
 #include <memory>
 #include <string>
 
@@ -96,7 +96,18 @@ namespace amr_control
      * @brief Check if publishing is active
      * @return True if publishing is active (either global or local)
      */
-    bool isPublishing() const { return global_costmap_obj_.active_ || local_costmap_obj_.active_; }
+    bool isPublishing() const { return global_costmap_obj_.active_ || local_costmap_obj_.active_ || cmd_vel_publishing_active_; }
+
+    /**
+     * @brief Start publishing cmd_vel at a fixed rate using ros::WallTimer
+     * @param rate Publishing rate in Hz (default: 20.0 Hz)
+     */
+     void startCmdVelPublishing(double rate = 20.0);
+
+     /**
+      * @brief Stop publishing cmd_vel
+      */
+     void stopCmdVelPublishing();
 
   private:
     /**
@@ -142,11 +153,27 @@ namespace amr_control
      */
     void localCostmapTimerCallback(const ros::TimerEvent &event);
 
+    /**
+     * @brief Publish cmd_vel command
+     */
+    void publishCmdVel();
+
+    /**
+     * @brief WallTimer callback for publishing cmd_vel
+     * @param event WallTimer event
+     */
+    void cmdVelTimerCallback(const ros::WallTimerEvent &event);
+
     ros::NodeHandle nh_;
     robot::move_base_core::BaseNavigation::Ptr move_base_ptr_;
 
     CostmapObject global_costmap_obj_;
     CostmapObject local_costmap_obj_;
+    ros::Publisher cmd_vel_pub_;
+    
+    // ros::WallTimer for cmd_vel publishing
+    ros::WallTimer cmd_vel_timer_;
+    bool cmd_vel_publishing_active_;
   };
 
 } // namespace amr_control
diff --git a/Controllers/Packages/amr_control/include/amr_control/amr_subcriber.h b/Controllers/Packages/amr_control/include/amr_control/amr_subcriber.h
new file mode 100644
index 0000000..a707f76
--- /dev/null
+++ b/Controllers/Packages/amr_control/include/amr_control/amr_subcriber.h
@@ -0,0 +1,28 @@
+#ifndef __AMR_SUBCRIBER_H_INCLUDED_
+#define __AMR_SUBCRIBER_H_INCLUDED_
+
+#include <ros/ros.h>
+#include <robot/robot.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <move_base_core/navigation.h>
+#include <memory>
+#include <string>
+
+namespace amr_control
+{
+  class AmrSubscriber
+  {
+  public:
+    AmrSubscriber(ros::NodeHandle &nh, robot::move_base_core::BaseNavigation::Ptr move_base_ptr);
+    virtual ~AmrSubscriber();
+
+  private:
+    void moveBaseSimpleCallback(const geometry_msgs::PoseStamped::ConstPtr &msg);
+    
+    ros::NodeHandle nh_;
+    ros::Subscriber move_base_simple_sub_;
+    robot::move_base_core::BaseNavigation::Ptr move_base_ptr_;
+  };
+} // namespace amr_control
+
+#endif // __AMR_SUBCRIBER_H_INCLUDED_
\ No newline at end of file
diff --git a/Controllers/Packages/amr_control/include/amr_control/sensor_converter.h b/Controllers/Packages/amr_control/include/amr_control/sensor_converter.h
index 132acb9..aae6212 100644
--- a/Controllers/Packages/amr_control/include/amr_control/sensor_converter.h
+++ b/Controllers/Packages/amr_control/include/amr_control/sensor_converter.h
@@ -1,6 +1,7 @@
 #ifndef __SENSOR_CONVERTER_H_INCLUDED_
 #define __SENSOR_CONVERTER_H_INCLUDED_
 #include <ros/ros.h>
+#include <robot/robot.h>
 #include <nav_msgs/OccupancyGrid.h>
 #include <nav_msgs/GetMap.h>
 #include <sensor_msgs/LaserScan.h>
@@ -10,8 +11,6 @@
 #include <robot_sensor_msgs/PointCloud2.h>
 #include <robot_geometry_msgs/Point.h>
 #include <robot_geometry_msgs/PoseStamped.h>
-#include <robot/console.h>
-#include <robot/time.h>
 #include <move_base_core/navigation.h>
 #include <memory>
 
diff --git a/Controllers/Packages/amr_control/include/amr_control/tf_converter.h b/Controllers/Packages/amr_control/include/amr_control/tf_converter.h
index 3a8dac1..467be91 100644
--- a/Controllers/Packages/amr_control/include/amr_control/tf_converter.h
+++ b/Controllers/Packages/amr_control/include/amr_control/tf_converter.h
@@ -2,8 +2,7 @@
 #define __TF_CONVERTER_H_INCLUDED_
 
 #include <ros/ros.h>
-#include <robot/console.h>
-#include <robot/time.h>
+#include <robot/robot.h>
 #include <tf2_ros/transform_listener.h>
 #include <tf3/buffer_core.h>
 #include <tf3/exceptions.h>
diff --git a/Controllers/Packages/amr_control/package.xml b/Controllers/Packages/amr_control/package.xml
index 66f35a8..594b37b 100644
--- a/Controllers/Packages/amr_control/package.xml
+++ b/Controllers/Packages/amr_control/package.xml
@@ -91,14 +91,15 @@
   <build_depend>robot_cpp</build_depend>
   <build_depend>robot_nav_2d_utils</build_depend>
   <build_depend>move_base_core</build_depend>
-
+  <build_depend>robot_nav_msgs</build_depend>
   <build_export_depend>robot_cpp</build_export_depend>
   <build_export_depend>robot_nav_2d_utils</build_export_depend>
   <build_export_depend>move_base_core</build_export_depend>
-
+  <build_export_depend>robot_nav_msgs</build_export_depend>
   <exec_depend>robot_cpp</exec_depend>
   <exec_depend>robot_nav_2d_utils</exec_depend>
   <exec_depend>move_base_core</exec_depend>
+  <exec_depend>robot_nav_msgs</exec_depend>
   <!-- The export tag contains other, unspecified, tags -->
   <export>
     <!-- Other tools can request additional information be placed here -->
diff --git a/Controllers/Packages/amr_control/src/amr_control.cpp b/Controllers/Packages/amr_control/src/amr_control.cpp
index 4f5a5b8..65042e5 100644
--- a/Controllers/Packages/amr_control/src/amr_control.cpp
+++ b/Controllers/Packages/amr_control/src/amr_control.cpp
@@ -1,8 +1,6 @@
 #include "amr_control/amr_control.h"
 
-#include <robot/plugin_loader_helper.h>
-#include <robot/console.h>  
-#include <robot/node_handle.h>
+#include <robot/robot.h>
 #include <tf3/buffer_core.h>
 #include <geometry_msgs/Vector3.h>
 #include <xmlrpcpp/XmlRpcValue.h>
@@ -79,11 +77,12 @@ namespace amr_control
       vda_5050_client_api_ptr_.reset();
     }
 
-    // Stop costmap publishing
-    if (costmap_publisher_ptr_)
+    // Stop costmap and cmd_vel publishing
+    if (amr_publisher_ptr_)
     {
-      costmap_publisher_ptr_->stopPublishing();
-      costmap_publisher_ptr_.reset();
+      amr_publisher_ptr_->stopPublishing();
+      amr_publisher_ptr_->stopCmdVelPublishing();
+      amr_publisher_ptr_.reset();
     }
   }
 
@@ -243,8 +242,7 @@ namespace amr_control
       robot::PluginLoaderHelper loader;
       std::string path_file_so = loader.findLibraryPath("MoveBase");
       move_base_loader_ = boost::dll::import_alias<robot::move_base_core::BaseNavigation::Ptr()>(
-        path_file_so,
-        "MoveBase", boost::dll::load_mode::append_decorations);
+        path_file_so, "MoveBase", boost::dll::load_mode::append_decorations);
       
       robot::log_info("[%s:%d]\n Creating MoveBase instance...", __FILE__, __LINE__);
       move_base_ptr_ = move_base_loader_();
@@ -253,7 +251,8 @@ namespace amr_control
 
       // Initialize costmap publisher for RViz visualization
       robot::log_info("[%s:%d]\n Initializing costmap publisher...", __FILE__, __LINE__);
-      costmap_publisher_ptr_ = std::make_shared<amr_control::AmrPublisher>(nh, move_base_ptr_);
+      amr_publisher_ptr_ = std::make_shared<amr_control::AmrPublisher>(nh, move_base_ptr_);
+      amr_subscriber_ptr_ = std::make_shared<amr_control::AmrSubscriber>(nh, move_base_ptr_);
       
       ros::Rate r(3);
       do
@@ -330,10 +329,16 @@ namespace amr_control
         // Start publishing costmaps to RViz
         // Global costmap: 1 Hz, Local costmap: 5 Hz
 
-        if (costmap_publisher_ptr_)
+        if (amr_publisher_ptr_)
         {
           robot::log_info("[%s:%d]\n Starting costmap publishing to RViz...", __FILE__, __LINE__);
-          costmap_publisher_ptr_->startPublishing(1.0, 5.0);
+          amr_publisher_ptr_->startPublishing(1.0, 5.0);
+          
+          // Start publishing cmd_vel using WallTimer
+          double cmd_vel_rate = 20.0;  // Default: 20 Hz
+          nh.param("cmd_vel_publish_rate", cmd_vel_rate, cmd_vel_rate);
+          robot::log_info("[%s:%d]\n Starting cmd_vel publishing at %.2f Hz...", __FILE__, __LINE__, cmd_vel_rate);
+          amr_publisher_ptr_->startCmdVelPublishing(cmd_vel_rate);
         }
       }
     }
diff --git a/Controllers/Packages/amr_control/src/amr_opc_ua_server_api.cpp b/Controllers/Packages/amr_control/src/amr_opc_ua_server_api.cpp
index 370818f..7ce1e2b 100644
--- a/Controllers/Packages/amr_control/src/amr_opc_ua_server_api.cpp
+++ b/Controllers/Packages/amr_control/src/amr_opc_ua_server_api.cpp
@@ -8,7 +8,7 @@
 #include <angles/angles.h>
 
 // pnkx core
-#include <robot/time.h>
+#include <robot/robot.h>
 #include <robot_nav_2d_utils/conversions.h>
 #include <robot_nav_2d_msgs/Twist2D.h>
 
diff --git a/Controllers/Packages/amr_control/src/amr_publiser.cpp b/Controllers/Packages/amr_control/src/amr_publiser.cpp
index b5ae73c..9e83fd2 100644
--- a/Controllers/Packages/amr_control/src/amr_publiser.cpp
+++ b/Controllers/Packages/amr_control/src/amr_publiser.cpp
@@ -1,8 +1,10 @@
 #include <amr_control/amr_publiser.h>
-#include <robot/console.h>
+#include <robot/robot.h>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 #include <robot_costmap_2d/cost_values.h>
 #include <boost/bind.hpp>
+#include <robot_nav_2d_msgs/Twist2DStamped.h>
+#include <geometry_msgs/Twist.h>
 
 namespace amr_control
 {
@@ -14,11 +16,11 @@ AmrPublisher::AmrPublisher(ros::NodeHandle &nh, robot::move_base_core::BaseNavig
   // Initialize default topic names
   std::string global_costmap_topic = "/global_costmap/costmap";
   std::string local_costmap_topic = "/local_costmap/costmap";
-  
+  std::string cmd_vel_topic = "/cmd_vel";
   // Get topic names from parameter server if available
   nh_.param("global_costmap_topic", global_costmap_topic, global_costmap_topic);
   nh_.param("local_costmap_topic", local_costmap_topic, local_costmap_topic);
-
+  nh_.param("cmd_vel_topic", cmd_vel_topic, cmd_vel_topic);
   // Initialize publishers with callback for new subscriptions
   global_costmap_obj_.pub_ = nh_.advertise<nav_msgs::OccupancyGrid>(
     global_costmap_topic, 1, 
@@ -32,6 +34,10 @@ AmrPublisher::AmrPublisher(ros::NodeHandle &nh, robot::move_base_core::BaseNavig
   local_costmap_obj_.update_pub_ = nh_.advertise<map_msgs::OccupancyGridUpdate>(local_costmap_topic + "_updates", 1);
   local_costmap_obj_.footprint_pub_ = nh_.advertise<geometry_msgs::PolygonStamped>(local_costmap_topic + "/footprint", 1);
   
+
+  cmd_vel_pub_ = nh_.advertise<geometry_msgs::Twist>(cmd_vel_topic, 1);
+  cmd_vel_publishing_active_ = false;
+  robot::log_info("[AmrPublisher] Initialized. Cmd vel topic: %s", cmd_vel_topic.c_str());
   // Initialize topic names in CostmapObject
   global_costmap_obj_.topic_ = global_costmap_topic;
   local_costmap_obj_.topic_ = local_costmap_topic;
@@ -45,11 +51,13 @@ AmrPublisher::AmrPublisher(ros::NodeHandle &nh, robot::move_base_core::BaseNavig
                   global_costmap_topic.c_str(), local_costmap_topic.c_str());
   robot::log_info("[AmrPublisher] Global footprint topic: %s/footprint, Local footprint topic: %s/footprint",
                   global_costmap_topic.c_str(), local_costmap_topic.c_str());
+              
 }
 
 AmrPublisher::~AmrPublisher()
 {
   stopPublishing();
+  stopCmdVelPublishing();
 }
 
 void AmrPublisher::convertToNavOccupancyGrid(const robot_nav_msgs::OccupancyGrid &robot_grid, 
@@ -331,5 +339,71 @@ void AmrPublisher::onNewSubscriptionGlobal(const ros::SingleSubscriberPublisher&
   publishGlobalCostmap();
 }
 
+void AmrPublisher::publishCmdVel()
+{
+  if(!move_base_ptr_)
+  {
+    return;
+  }
+
+  if(!move_base_ptr_->getFeedback()->is_ready)
+  {
+    return;
+  }
+
+  robot_nav_2d_msgs::Twist2DStamped twist = move_base_ptr_->getTwist();
+  if(twist.header.stamp < robot::Time::now() - robot::Duration(0.05))
+  {
+    return;
+  }
+
+  geometry_msgs::Twist cmd_vel;
+  cmd_vel.linear.x = twist.velocity.x;
+  cmd_vel.linear.y = twist.velocity.y;
+  cmd_vel.angular.z = twist.velocity.theta;
+  cmd_vel_pub_.publish(cmd_vel);
+}
+
+void AmrPublisher::startCmdVelPublishing(double rate)
+{
+  stopCmdVelPublishing(); // Stop any existing publishing first
+
+  if (rate <= 0.0)
+  {
+    robot::log_warning("[AmrPublisher] Invalid cmd_vel publishing rate: %.2f Hz. Must be > 0", rate);
+    return;
+  }
+
+  ros::WallDuration period(1.0 / rate);
+  cmd_vel_timer_ = nh_.createWallTimer(
+    period,
+    &AmrPublisher::cmdVelTimerCallback,
+    this,
+    false  // oneshot = false
+  );
+
+  cmd_vel_publishing_active_ = true;
+  robot::log_info("[AmrPublisher] Started publishing cmd_vel at %.2f Hz using ros::WallTimer", rate);
+}
+
+void AmrPublisher::stopCmdVelPublishing()
+{
+  if (cmd_vel_publishing_active_)
+  {
+    cmd_vel_timer_.stop();
+    cmd_vel_publishing_active_ = false;
+    robot::log_info("[AmrPublisher] Stopped publishing cmd_vel");
+  }
+}
+
+void AmrPublisher::cmdVelTimerCallback(const ros::WallTimerEvent &event)
+{
+  (void)event;
+  if (cmd_vel_publishing_active_)
+  {
+    publishCmdVel();
+  }
+}
+
 } // namespace amr_control
 
diff --git a/Controllers/Packages/amr_control/src/amr_subcriber.cpp b/Controllers/Packages/amr_control/src/amr_subcriber.cpp
new file mode 100644
index 0000000..b2681cb
--- /dev/null
+++ b/Controllers/Packages/amr_control/src/amr_subcriber.cpp
@@ -0,0 +1,28 @@
+#include "amr_control/amr_subcriber.h"
+#include <geometry_msgs/PoseStamped.h>
+
+amr_control::AmrSubscriber::AmrSubscriber(ros::NodeHandle &nh, robot::move_base_core::BaseNavigation::Ptr move_base_ptr)
+  : nh_(nh), move_base_ptr_(move_base_ptr)
+{   
+    robot::log_info("[%s:%d] Initializing AmrSubscriber namespace: %s", __FILE__, __LINE__, nh_.getNamespace().c_str());
+    ros::NodeHandle nh_move_base_simple;
+    move_base_simple_sub_ = nh_move_base_simple.subscribe("/move_base_simple/goal", 1, &AmrSubscriber::moveBaseSimpleCallback, this);
+}
+
+amr_control::AmrSubscriber::~AmrSubscriber()
+{
+}
+
+void amr_control::AmrSubscriber::moveBaseSimpleCallback(const geometry_msgs::PoseStamped::ConstPtr &msg)
+{
+  robot_geometry_msgs::PoseStamped goal;
+  goal.header.stamp = robot::Time::now();
+  goal.header.frame_id = msg->header.frame_id;
+  goal.pose.position.x = msg->pose.position.x;
+  goal.pose.position.y = msg->pose.position.y;
+  goal.pose.orientation.x = msg->pose.orientation.x;
+  goal.pose.orientation.y = msg->pose.orientation.y;
+  goal.pose.orientation.z = msg->pose.orientation.z;
+  goal.pose.orientation.w = msg->pose.orientation.w;
+  move_base_ptr_->moveTo(goal, 0.02, 0.02);
+}
\ No newline at end of file
diff --git a/Controllers/Packages/amr_control/src/amr_vda_5050_client_api.cpp b/Controllers/Packages/amr_control/src/amr_vda_5050_client_api.cpp
index cc98291..4282871 100644
--- a/Controllers/Packages/amr_control/src/amr_vda_5050_client_api.cpp
+++ b/Controllers/Packages/amr_control/src/amr_vda_5050_client_api.cpp
@@ -3,6 +3,9 @@
 #include <functional>
 #include <optional>
 #include "vda5050_msgs/Order.h"
+#include "robot_nav_msgs/Path.h"
+#include "robot_geometry_msgs/Pose2D.h"
+#include "robot_nav_2d_utils/conversions.h"
 
 namespace amr_control
 {
@@ -141,17 +144,17 @@ namespace amr_control
         if (!paths.empty())
           return;
 
-        std::vector<geometry_msgs::Pose2D> poses;
+        std::vector<robot_geometry_msgs::Pose2D> poses;
         for (auto &p : paths)
         {
-          geometry_msgs::Pose2D p_2d;
+          robot_geometry_msgs::Pose2D p_2d;
           p_2d.x = p.getX();
           p_2d.y = p.getY();
           p_2d.theta = p.getYaw();
           poses.push_back(p_2d);
         }
-        // const nav_msgs::Path path = nav_2d_utils::poses2DToPath(poses, "map", ros::Time::now());
-        // goal = path.poses.back();
+        const robot_nav_msgs::Path path = robot_nav_2d_utils::poses2DToPath(poses, "map", robot::Time::now());
+        goal = path.poses.back();
         // VDA5050ClientAPI::plan_pub_.publish(path);
       }
       else if (global_plan_msg_type_ == std::string("vda5050_msgs::Order"))
@@ -226,14 +229,20 @@ namespace amr_control
         goal.header.frame_id = "map";
         goal.pose.position.x = position.x;
         goal.pose.position.y = position.y;
-        // tf3::Transform transform;
-        // transform.setRotation(tf3::Quaternion(0.0, 0.0, position.theta));
-        // goal = tf3::toMsg(transform);
+        goal.pose.position.z = 0.0;
+        
+        // Convert theta (yaw) to Quaternion using setRPY
+        // setRPY(roll, pitch, yaw) - for 2D navigation, only yaw is used
+        tf3::Quaternion quat;
+        quat.setRPY(0.0, 0.0, position.theta);
+        goal.pose.orientation.x = quat.x();
+        goal.pose.orientation.y = quat.y();
+        goal.pose.orientation.z = quat.z();
+        goal.pose.orientation.w = quat.w();
       }
       else
         return;
 
-      ros::Duration(0.1).sleep();
       VDA5050ClientAPI::move_base_ptr_->moveTo(goal);
       cmd_vel_max_.x = 0.2;
       cmd_vel_max_.theta = 0.5;
diff --git a/Controllers/Packages/amr_startup/rviz/navigation.rviz b/Controllers/Packages/amr_startup/rviz/navigation.rviz
index a1f3413..d925eba 100644
--- a/Controllers/Packages/amr_startup/rviz/navigation.rviz
+++ b/Controllers/Packages/amr_startup/rviz/navigation.rviz
@@ -7,12 +7,8 @@ Panels:
         - /Global Options1
         - /Grid1/Offset1
         - /TF1/Frames1
-        - /Global Map1
-        - /Local Map1/Costmap1
-        - /Local Map1/Trajectory1
-        - /Local Map1/Trajectory1/transform plan1
       Splitter Ratio: 0.37295082211494446
-    Tree Height: 422
+    Tree Height: 682
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -69,18 +65,118 @@ Visualization Manager:
     - Alpha: 0.5
       Class: rviz/RobotModel
       Collision Enabled: true
-      Enabled: false
+      Enabled: true
       Links:
         All Links Enabled: true
         Expand Joint Details: false
         Expand Link Details: false
         Expand Tree: false
         Link Tree Style: Links in Alphabetic Order
+        back_nanoscan3_base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        back_nanoscan3_sensor_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        base_footprint:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        bl_caster_rotation_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        bl_caster_wheel_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        br_caster_rotation_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        br_caster_wheel_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        camera_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        fl_caster_rotation_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        fl_caster_wheel_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        fr_caster_rotation_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        fr_caster_wheel_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        front_nanoscan3_base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        front_nanoscan3_sensor_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        imu_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        imu_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        left_wheel_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        lifting_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        right_wheel_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        surface:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
       Name: RobotModel
       Robot Description: robot_description
       TF Prefix: ""
       Update Interval: 0
-      Value: false
+      Value: true
       Visual Enabled: true
     - Class: rviz/TF
       Enabled: true
@@ -388,7 +484,7 @@ Visualization Manager:
           Enabled: true
           Name: Footprint
           Queue Size: 10
-          Topic: /amr_node/local_costmap/footprint
+          Topic: /local_costmap/costmap/footprint
           Unreliable: false
           Value: true
         - Alpha: 1
@@ -596,7 +692,7 @@ Visualization Manager:
   Value: true
   Views:
     Current:
-      Angle: -3.135005474090576
+      Angle: -3.150002956390381
       Class: rviz/TopDownOrtho
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
@@ -606,10 +702,10 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Scale: -168.75274658203125
+      Scale: -62.776824951171875
       Target Frame: base_link
-      X: -0.49439820647239685
-      Y: 0.196189746260643
+      X: 1.189023494720459
+      Y: 0.44669991731643677
     Saved:
       - Angle: -34.55989074707031
         Class: rviz/TopDownOrtho
@@ -628,10 +724,10 @@ Visualization Manager:
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 573
+  Height: 833
   Hide Left Dock: false
   Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd00000004000000000000024f000001e3fc0200000008fb000000100044006900730070006c006100790073010000003d000001e3000000c900fffffffb0000001200530065006c0065006300740069006f006e000000003d000002590000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001b500000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb0000000a0049006d006100670065000000003d0000039e0000000000000000fb0000000c00430061006d006500720061020000007c0000013600000280000001e0fb0000000c00430061006d0065007200610200000644000000b7000002c1000002f9000000010000010f00000259fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000259000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b20000000000000000000000020000041800000168fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007520000003efc0100000002fb0000000800540069006d00650000000000000007520000041800fffffffb0000000800540069006d00650100000000000004500000000000000000000001e0000001e300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd000000040000000000000156000002e7fc0200000008fb000000100044006900730070006c006100790073010000003d000002e7000000c900fffffffb0000001200530065006c0065006300740069006f006e000000003d000002590000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001b500000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb0000000a0049006d006100670065000000003d0000039e0000000000000000fb0000000c00430061006d006500720061020000007c0000013600000280000001e0fb0000000c00430061006d0065007200610200000644000000b7000002c1000002f9000000010000010f00000259fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000259000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b20000000000000000000000020000041800000168fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007520000003efc0100000002fb0000000800540069006d00650000000000000007520000041800fffffffb0000000800540069006d00650100000000000004500000000000000000000002d9000002e700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
diff --git a/pnkx_nav_core b/pnkx_nav_core
index 145fb20..57b77ac 160000
--- a/pnkx_nav_core
+++ b/pnkx_nav_core
@@ -1 +1 @@
-Subproject commit 145fb2088ea8145d6a51927142b5df00409a0aa6
+Subproject commit 57b77ac14b144acddccc95cc9dd7e8d4d58728dc