update

2026-01-12 15:49:25 +07:00
parent f5e7e1f1e0
commit 145fb2088e
29 changed files with 3091 additions and 771 deletions
--- a/src/Navigations/Cores/move_base_core/README.md
+++ b/src/Navigations/Cores/move_base_core/README.md
@@ -1,2 +1,243 @@
 # move_base_core

+## Tổng quan
+
+`move_base_core` là một thư viện header-only C++ cung cấp các interface và cấu trúc dữ liệu cốt lõi cho hệ thống navigation của robot, tương tự như `move_base_msgs` trong ROS nhưng độc lập, không phụ thuộc vào ROS. Thư viện này định nghĩa:
+
+- **Interface chính**: `BaseNavigation` - giao diện trừu tượng cho các module navigation
+- **Trạng thái navigation**: Enum `State` mô tả các trạng thái của quá trình navigation
+- **Feedback structure**: `NavFeedback` để theo dõi trạng thái navigation
+- **Planner data**: `PlannerDataOutput` cho dữ liệu đầu ra từ planner
+- **Utility functions**: Các hàm hỗ trợ chuyển đổi và tính toán
+
+## Mục đích
+
+Thư viện này được thiết kế để:
+
+- Cung cấp API chuẩn hóa cho navigation system không sử dụng ROS
+- Định nghĩa interface chung cho các implementation navigation khác nhau
+- Hỗ trợ các chức năng navigation cơ bản: di chuyển đến điểm, docking, quay, tạm dừng
+- Cung cấp cơ chế feedback và trạng thái để theo dõi quá trình navigation
+- Đảm bảo tính nhất quán và khả năng mở rộng cho các module navigation
+
+## Các thành phần chính
+
+### 1. `BaseNavigation` - Interface chính cho Navigation
+
+`BaseNavigation` là abstract class định nghĩa tất cả các phương thức cần thiết cho một hệ thống navigation hoàn chỉnh.
+
+**Chức năng chính:**
+
+- **Khởi tạo**: `initialize()` - khởi tạo hệ thống navigation với TF listener
+- **Quản lý footprint**: `setRobotFootprint()`, `getRobotFootprint()` - thiết lập và lấy hình dạng robot
+- **Quản lý dữ liệu cảm biến**: 
+  - Thêm/xóa static maps, laser scans, point clouds
+  - Quản lý nhiều nguồn dữ liệu với tên riêng biệt
+- **Quản lý odometry**: `addOdometry()` - cập nhật thông tin vị trí robot
+- **Navigation commands**:
+  - `moveTo()` - di chuyển đến một điểm đích
+  - `dockTo()` - docking đến marker (ví dụ: trạm sạc)
+  - `moveStraightTo()` - di chuyển thẳng đến đích
+  - `rotateTo()` - quay tại chỗ đến hướng mong muốn
+- **Điều khiển motion**:
+  - `pause()` - tạm dừng di chuyển
+  - `resume()` - tiếp tục di chuyển
+  - `cancel()` - hủy mục tiêu hiện tại
+  - `setTwistLinear()`, `setTwistAngular()` - điều khiển vận tốc
+- **Lấy thông tin**:
+  - `getRobotPose()` - lấy vị trí hiện tại của robot
+  - `getTwist()` - lấy vận tốc hiện tại
+  - `getFeedback()` - lấy feedback về trạng thái navigation
+  - `getGlobalData()`, `getLocalData()` - lấy dữ liệu từ planner
+
+**Đặc điểm:**
+
+- Tất cả các phương thức đều là pure virtual, đảm bảo implementation đầy đủ
+- Hỗ trợ nhiều nguồn dữ liệu cảm biến với tên riêng biệt
+- Cung cấp cả PoseStamped và Pose2D cho linh hoạt
+- Thread-safe design với shared pointers
+
+### 2. `State` - Trạng thái Navigation
+
+Enum `State` mô tả các trạng thái khác nhau trong quá trình navigation.
+
+**Các trạng thái chính:**
+
+- **PENDING**: Chờ xử lý, mục tiêu đã được gửi nhưng chưa bắt đầu
+- **ACTIVE**: Đang hoạt động, navigation đang được thực thi
+- **PLANNING**: Đang lập kế hoạch đường đi
+- **CONTROLLING**: Đang điều khiển robot di chuyển theo plan
+- **CLEARING**: Đang dọn dẹp costmap, thực hiện recovery behaviors
+- **PAUSED**: Tạm dừng navigation
+- **SUCCEEDED**: Hoàn thành thành công
+- **ABORTED**: Bị lỗi, không thể hoàn thành
+- **PREEMPTED**: Bị hủy bởi mục tiêu mới
+- **REJECTED**: Bị từ chối bởi planner hoặc controller
+- **LOST**: Mất trạng thái, không xác định được vị trí
+
+**Ứng dụng:**
+
+- Theo dõi tiến trình navigation
+- Debugging và logging
+- Hiển thị trạng thái trong UI
+- Điều khiển state machine của navigation system
+
+### 3. `NavFeedback` - Feedback Navigation
+
+Cấu trúc `NavFeedback` cung cấp thông tin chi tiết về trạng thái navigation hiện tại.
+
+**Các thành phần:**
+
+- **navigation_state**: Trạng thái hiện tại của navigation (State enum)
+- **feed_back_str**: Chuỗi mô tả hoặc thông điệp debug
+- **current_pose**: Vị trí hiện tại của robot (Pose2D)
+- **goal_checked**: Cờ xác nhận mục tiêu đã được kiểm tra
+- **is_ready**: Cờ cho biết robot đã sẵn sàng nhận lệnh
+
+**Ứng dụng:**
+
+- Cập nhật trạng thái real-time cho người dùng
+- Debugging và monitoring
+- Ghi log quá trình navigation
+- Tích hợp với hệ thống giám sát
+
+### 4. `PlannerDataOutput` - Dữ liệu Planner
+
+Cấu trúc `PlannerDataOutput` chứa dữ liệu đầu ra từ planner.
+
+**Các thành phần:**
+
+- **plan**: Đường đi đã được lập kế hoạch (Path2D)
+- **costmap**: Bản đồ chi phí (OccupancyGrid)
+- **costmap_update**: Cập nhật bản đồ chi phí (OccupancyGridUpdate)
+
+**Ứng dụng:**
+
+- Lấy kế hoạch đường đi từ global/local planner
+- Xem costmap để debugging
+- Cập nhật costmap động
+- Visualization và monitoring
+
+### 5. Utility Functions - Hàm hỗ trợ
+
+Các hàm tiện ích hỗ trợ các thao tác phổ biến.
+
+**`toString(State state)`**:
+
+- Chuyển đổi enum State sang chuỗi mô tả
+- Hữu ích cho logging, debugging, hiển thị UI
+- Trả về tên trạng thái dễ đọc (ví dụ: "PLANNING", "CONTROLLING")
+
+**`offset_goal()`**:
+
+- Tạo mục tiêu mới bằng cách offset từ vị trí hiện tại
+- Hỗ trợ offset dọc theo hướng heading của robot
+- Có hai overload: từ Pose2D và từ PoseStamped
+- Hữu ích cho các tác vụ như di chuyển tiến/lùi một khoảng cách cố định
+
+## Ứng dụng
+
+### Trong Navigation System
+
+`move_base_core` là nền tảng cho các implementation navigation như `move_base`:
+
+- Định nghĩa contract mà tất cả navigation modules phải tuân theo
+- Cho phép thay thế implementation mà không thay đổi code sử dụng
+- Hỗ trợ plugin system cho các navigation strategies khác nhau
+
+### Trong User Applications
+
+Các ứng dụng sử dụng navigation system:
+
+- Gọi các phương thức `moveTo()`, `dockTo()`, `rotateTo()` để điều khiển robot
+- Theo dõi `NavFeedback` để biết trạng thái navigation
+- Sử dụng `getRobotPose()` để lấy vị trí hiện tại
+- Quản lý dữ liệu cảm biến thông qua các phương thức add/remove
+
+### Trong Monitoring Systems
+
+Hệ thống giám sát và logging:
+
+- Sử dụng `State` để hiển thị trạng thái navigation
+- Đọc `NavFeedback` để cập nhật UI real-time
+- Sử dụng `toString()` để format log messages
+- Lấy `PlannerDataOutput` để visualize costmap và plan
+
+## Best Practices
+
+### Implementation BaseNavigation
+
+- Implement đầy đủ tất cả pure virtual methods
+- Đảm bảo thread-safety cho các phương thức public
+- Cập nhật `nav_feedback_` một cách nhất quán
+- Xử lý lỗi một cách graceful và cập nhật state phù hợp
+
+### Sử dụng State
+
+- Luôn kiểm tra state trước khi thực hiện các thao tác
+- Sử dụng `toString()` cho logging thay vì hardcode strings
+- Xử lý các trạng thái lỗi (ABORTED, REJECTED) một cách phù hợp
+
+### Quản lý Feedback
+
+- Cập nhật `navigation_state` ngay khi state thay đổi
+- Cung cấp `feed_back_str` mô tả rõ ràng cho debugging
+- Đảm bảo `current_pose` được cập nhật thường xuyên
+- Sử dụng `is_ready` để kiểm tra robot sẵn sàng nhận lệnh
+
+### Quản lý Dữ liệu Cảm biến
+
+- Sử dụng tên rõ ràng, duy nhất cho mỗi nguồn dữ liệu
+- Xóa dữ liệu không còn sử dụng để tiết kiệm bộ nhớ
+- Kiểm tra dữ liệu tồn tại trước khi sử dụng
+- Cập nhật dữ liệu thường xuyên để đảm bảo tính chính xác
+
+## Tính năng nổi bật
+
+### Header-Only Library
+
+- Không cần compile, chỉ cần include header files
+- Dễ dàng tích hợp vào các project khác
+- Không có runtime dependencies
+
+### ROS-Independent
+
+- Hoàn toàn độc lập với ROS
+- Có thể sử dụng trong các ứng dụng C++ thuần
+- Tương thích với các framework khác
+
+### Extensible Design
+
+- Dễ dàng mở rộng với các phương thức mới
+- Hỗ trợ nhiều implementation khác nhau
+- Plugin-friendly architecture
+
+### Type-Safe
+
+- Sử dụng strong typing với enums và structures
+- Compile-time type checking
+- Giảm thiểu lỗi runtime
+
+## Dependencies
+
+`move_base_core` phụ thuộc vào:
+
+- **tf3**: Transform library cho coordinate frames
+- **robot_time**: Time utilities
+- **robot_geometry_msgs**: Geometry message types
+- **robot_nav_2d_msgs**: 2D navigation message types
+- **robot_nav_msgs**: Navigation message types
+- **robot_sensor_msgs**: Sensor message types
+- **robot_map_msgs**: Map message types
+- **robot_protocol_msgs**: Protocol message types
+
+## Tóm tắt
+
+`move_base_core` là nền tảng quan trọng cho navigation system, cung cấp:
+
+- Interface chuẩn hóa cho navigation modules
+- Cấu trúc dữ liệu cho state và feedback
+- Utility functions hỗ trợ các thao tác phổ biến
+- Design linh hoạt, dễ mở rộng và không phụ thuộc ROS
+
+Thư viện này đảm bảo tính nhất quán và khả năng tương thích giữa các component navigation khác nhau, tạo nền tảng vững chắc cho việc phát triển các ứng dụng navigation phức tạp.
--- a/src/Navigations/Cores/move_base_core/include/move_base_core/navigation.h
+++ b/src/Navigations/Cores/move_base_core/include/move_base_core/navigation.h
@@ -8,9 +8,12 @@
 #include <robot_geometry_msgs/PoseStamped.h>
 #include <robot_geometry_msgs/Pose2D.h>
 #include <robot_geometry_msgs/Vector3.h>
+#include <robot_geometry_msgs/PolygonStamped.h>
 #include <robot_nav_2d_msgs/Twist2DStamped.h>
+#include <robot_nav_2d_msgs/Path2D.h>
 #include <robot_nav_msgs/Odometry.h>
 #include <robot_nav_msgs/OccupancyGrid.h>
+#include <robot_map_msgs/OccupancyGridUpdate.h>
 #include <robot_sensor_msgs/LaserScan.h>
 #include <robot_sensor_msgs/PointCloud.h>
 #include <robot_sensor_msgs/PointCloud2.h>
@@ -48,14 +51,28 @@ namespace move_base_core
 		PAUSED
 	};

-	// Feedback structure to describe current navigation status
+	/**
+	 * @brief Feedback structure to describe current navigation status
+	 */
 	struct NavFeedback
 	{
-		State navigation_state;							// Current navigation state
+		State navigation_state;						// Current navigation state
 		std::string feed_back_str;					// Description or debug message
-		robot_geometry_msgs::Pose2D current_pose; // Robot’s current pose in 2D
-		bool goal_checked;									// Whether the current goal is verified
-		bool is_ready;											// Robot is ready for commands
+		robot_geometry_msgs::Pose2D current_pose; 	// Robot’s current pose in 2D
+		bool goal_checked;							// Whether the current goal is verified
+		bool is_ready;								// Robot is ready for commands
+	};
+
+	/**
+	 * @brief Planner data output structure.
+	 */
+	struct PlannerDataOutput 
+	{
+		robot_nav_2d_msgs::Path2D plan;
+		robot_nav_msgs::OccupancyGrid costmap;
+		robot_map_msgs::OccupancyGridUpdate costmap_update;
+		bool is_costmap_updated;
+		robot_geometry_msgs::PolygonStamped footprint;
 	};

 	/**
@@ -460,27 +477,41 @@ namespace move_base_core

 		/**
 		 * @brief Get the robot’s twist.
-		 * @param twist Output parameter with the robot’s current twist.
-		 * @return True if twist was successfully retrieved.
+		 * @return The robot’s current twist.
 		 */
-		virtual bool getTwist(robot_nav_2d_msgs::Twist2DStamped &twist) = 0;
+		virtual robot_nav_2d_msgs::Twist2DStamped getTwist() = 0;

 		/**
 		 * @brief Get the navigation feedback.
 		 * @return Pointer to the navigation feedback.
 		 */
 		virtual NavFeedback *getFeedback() = 0;
-		

+		/**
+		 * @brief Get the global data.
+		 * @return The global data.
+		 */
+		virtual PlannerDataOutput getGlobalData() = 0;
+
+		/**
+		 * @brief Get the local data.
+		 * @return The local data.
+		 */
+		virtual PlannerDataOutput getLocalData() = 0;
+		
 	protected:
 		// Shared feedback data for navigation status tracking
 		std::shared_ptr<NavFeedback> nav_feedback_;
-		std::shared_ptr<robot_nav_2d_msgs::Twist2DStamped> twist_;
+		robot_nav_2d_msgs::Twist2DStamped twist_;
+		robot_nav_msgs::Odometry odometry_;
+		PlannerDataOutput global_data_;
+		PlannerDataOutput local_data_;

 		std::map<std::string, robot_nav_msgs::OccupancyGrid> static_maps_;
 		std::map<std::string, robot_sensor_msgs::LaserScan> laser_scans_;
 		std::map<std::string, robot_sensor_msgs::PointCloud> point_clouds_;
 		std::map<std::string, robot_sensor_msgs::PointCloud2> point_cloud2s_;
+
 		BaseNavigation() = default;
 	};

--- a/src/Navigations/Cores/move_base_core/package.xml
+++ b/src/Navigations/Cores/move_base_core/package.xml
@@ -2,12 +2,17 @@
  <name>move_base_core</name>
  <version>0.7.10</version>
  <description>
-    move_base_core is the second generation of the transform library, which lets
-    the user keep track of multiple coordinate frames over time. move_base_core
-    maintains the relationship between coordinate frames in a tree
-    structure buffered in time, and lets the user transform points,
-    vectors, etc between any two coordinate frames at any desired
-    point in time.
+    move_base_core provides the core interfaces and data structures for robot navigation
+    systems, independent of ROS. It includes:
+    - BaseNavigation: Abstract interface defining navigation operations (moveTo, dockTo, rotateTo, etc.)
+    - State: Navigation state enumeration (PLANNING, CONTROLLING, CLEARING, SUCCEEDED, etc.)
+    - NavFeedback: Structure for navigation status feedback
+    - PlannerDataOutput: Structure for planner output data
+    - Helper functions: State conversion, goal offsetting utilities
+    
+    This package serves as the foundation for implementing navigation modules, providing
+    a standardized API for goal-based navigation, motion control, and status reporting
+    in standalone C++ applications.
  </description>
  <author>Tully Foote</author>
  <author>Eitan Marder-Eppstein</author>
@@ -26,6 +31,7 @@
  <build_depend>robot_nav_2d_msgs</build_depend>
  <build_depend>robot_nav_msgs</build_depend>
  <build_depend>robot_sensor_msgs</build_depend>
+  <build_depend>robot_map_msgs</build_depend>

  <run_depend>tf3</run_depend>
  <run_depend>robot_time</run_depend>
@@ -34,4 +40,5 @@
  <run_depend>robot_nav_2d_msgs</run_depend>
  <run_depend>robot_nav_msgs</run_depend>
  <run_depend>robot_sensor_msgs</run_depend>
+  <run_depend>robot_map_msgs</run_depend>
 </package>
--- a/src/Navigations/Cores/robot_nav_core/include/robot_nav_core/base_local_planner.h
+++ b/src/Navigations/Cores/robot_nav_core/include/robot_nav_core/base_local_planner.h
@@ -55,10 +55,11 @@ namespace robot_nav_core
    using Ptr = std::shared_ptr<BaseLocalPlanner>;  
    /**
     * @brief  Given the current position, orientation, and velocity of the robot, compute velocity commands to send to the base
+     * @param velocity The velocity of the robot
     * @param cmd_vel Will be filled with the velocity command to be passed to the robot base
     * @return True if a valid velocity command was found, false otherwise
     */
-    virtual bool computeVelocityCommands(robot_geometry_msgs::Twist &cmd_vel) = 0;
+    virtual bool computeVelocityCommands(const robot_geometry_msgs::Twist &velocity, robot_geometry_msgs::Twist &cmd_vel) = 0;

    /**
     * @brief Swap object will be injected interjace robot_nav_core::BaseLocalPlanner
--- a/src/Navigations/Cores/robot_nav_core_adapter/include/robot_nav_core_adapter/local_planner_adapter.h
+++ b/src/Navigations/Cores/robot_nav_core_adapter/include/robot_nav_core_adapter/local_planner_adapter.h
@@ -81,10 +81,11 @@ namespace robot_nav_core_adapter

    /**
     * @brief  Given the current position, orientation, and velocity of the robot, compute velocity commands to send to the base
+     * @param velocity The velocity of the robot
     * @param cmd_vel Will be filled with the velocity command to be passed to the robot base
     * @return True if a valid velocity command was found, false otherwise
     */
-    bool computeVelocityCommands(robot_geometry_msgs::Twist &cmd_vel) override;
+    bool computeVelocityCommands(const robot_geometry_msgs::Twist &velocity, robot_geometry_msgs::Twist &cmd_vel) override;

    /**
     * @brief Swap object will be injected interjace robot_nav_core2::BaseLocalPlanner
@@ -181,9 +182,9 @@ namespace robot_nav_core_adapter
    bool has_active_goal_;

    /**
-     * @brief helper class for subscribing to odometry
+     * @brief The odometry of the robot
     */
-    std::shared_ptr<robot_nav_2d_utils::OdomSubscriber> odom_sub_;
+    robot_nav_msgs::Odometry odom_;

    // Plugin handling
    boost::function<robot_nav_core2::LocalPlanner::Ptr()> planner_loader_;
--- a/src/Navigations/Cores/robot_nav_core_adapter/src/local_planner_adapter.cpp
+++ b/src/Navigations/Cores/robot_nav_core_adapter/src/local_planner_adapter.cpp
@@ -165,7 +165,7 @@ namespace robot_nav_core_adapter
  /**
   * @brief Collect the additional information needed by robot_nav_core2 and call the child computeVelocityCommands
   */
-  bool LocalPlannerAdapter::computeVelocityCommands(robot_geometry_msgs::Twist &cmd_vel)
+  bool LocalPlannerAdapter::computeVelocityCommands(const robot_geometry_msgs::Twist &velocity, robot_geometry_msgs::Twist &cmd_vel)
  {
    boost::recursive_mutex::scoped_lock ecl(configuration_mutex_);
    if (!has_active_goal_)
@@ -181,11 +181,10 @@ namespace robot_nav_core_adapter
    }

    // Get the Velocity
-    robot_nav_2d_msgs::Twist2D velocity = odom_sub_->getTwist();
    robot_nav_2d_msgs::Twist2DStamped cmd_vel_2d;
    try
    {
-      cmd_vel_2d = planner_->computeVelocityCommands(pose2d, velocity);
+      cmd_vel_2d = planner_->computeVelocityCommands(pose2d, robot_nav_2d_utils::twist3Dto2D(velocity));
    }
    catch (const robot_nav_core2::PlannerException &e)
    {
@@ -262,10 +261,7 @@ namespace robot_nav_core_adapter

  robot_geometry_msgs::Twist LocalPlannerAdapter::getActualTwist()
  {
-    if (odom_sub_)
-      return robot_nav_2d_utils::twist2Dto3D(odom_sub_->getTwist());
-    else
-      throw std::runtime_error("Failed to get twist");
+    return odom_.twist.twist;
  }

  bool LocalPlannerAdapter::islock()
@@ -330,7 +326,7 @@ namespace robot_nav_core_adapter
    if (!getRobotPose(pose2d))
      return false;

-    robot_nav_2d_msgs::Twist2D velocity = odom_sub_->getTwist();
+    robot_nav_2d_msgs::Twist2D velocity = robot_nav_2d_utils::twist3Dto2D(odom_.twist.twist);
    bool ret = planner_->isGoalReached(pose2d, velocity);
    if (ret)
    {
--- a/src/Navigations/Packages/move_base/CMakeLists.txt
+++ b/src/Navigations/Packages/move_base/CMakeLists.txt
@@ -81,6 +81,7 @@ endif()
 # ========================================================
 add_library(${PROJECT_NAME} SHARED
  src/move_base.cpp
+  src/convert_data.cpp
 )

 if(BUILDING_WITH_CATKIN)
--- a/src/Navigations/Packages/move_base/README_ACTION_SERVER.md
+++ b/src/Navigations/Packages/move_base/README_ACTION_SERVER.md
@@ -0,0 +1,161 @@
+# MoveBaseActionServer - Hướng dẫn sử dụng
+
+## Tổng quan
+
+`MoveBaseActionServer` cung cấp một interface tương tự ROS Action Server, cho phép bạn:
+- Gửi goals và nhận feedback
+- Theo dõi tiến trình navigation
+- Hủy goals (cancel/preempt)
+- Nhận kết quả khi hoàn thành
+
+## So sánh với ROS Action Server
+
+| ROS Action Server | MoveBaseActionServer |
+|-------------------|---------------------|
+| `actionlib::ActionServer` | `move_base::MoveBaseActionServer` |
+| `as_->acceptNewGoal()` | `action_server.sendGoal()` |
+| `as_->setSucceeded()` | `action_server.setSucceeded()` |
+| `as_->setAborted()` | `action_server.setAborted()` |
+| `as_->setPreempted()` | `action_server.setPreempted()` |
+| `as_->publishFeedback()` | Tự động qua callback |
+| `as_->isNewGoalAvailable()` | `action_server.isNewGoalAvailable()` |
+| `as_->isPreemptRequested()` | `action_server.isPreemptRequested()` |
+
+## Cách sử dụng cơ bản
+
+### 1. Khởi tạo
+
+```cpp
+#include <move_base/move_base_action_server.h>
+
+// Tạo MoveBase instance (như bình thường)
+robot::move_base_core::BaseNavigation::Ptr move_base_ptr = ...;
+move_base_ptr->initialize(tf);
+
+// Tạo Action Server
+move_base::MoveBaseActionServer action_server(move_base_ptr, 20.0); // 20 Hz
+action_server.start();
+```
+
+### 2. Thiết lập Callbacks
+
+```cpp
+// Callback khi nhận goal mới
+action_server.setGoalCallback([](const move_base::MoveBaseActionGoal& goal) {
+    std::cout << "New goal: (" << goal.target_pose.pose.position.x 
+              << ", " << goal.target_pose.pose.position.y << ")" << std::endl;
+});
+
+// Callback cho feedback (gọi định kỳ)
+action_server.setFeedbackCallback([](const move_base::MoveBaseActionFeedback& feedback) {
+    std::cout << "State: " << robot::move_base_core::toString(
+        feedback.nav_feedback.navigation_state) << std::endl;
+    std::cout << "Distance to goal: " << feedback.distance_to_goal << " m" << std::endl;
+});
+
+// Callback khi goal hoàn thành
+action_server.setResultCallback([](const move_base::MoveBaseActionResult& result) {
+    if (result.success) {
+        std::cout << "Goal succeeded!" << std::endl;
+    } else {
+        std::cout << "Goal failed: " << result.message << std::endl;
+    }
+});
+
+// Callback khi goal bị preempt
+action_server.setPreemptCallback([]() {
+    std::cout << "Goal was preempted" << std::endl;
+});
+```
+
+### 3. Gửi Goal
+
+```cpp
+move_base::MoveBaseActionGoal goal;
+goal.target_pose.header.frame_id = "map";
+goal.target_pose.header.stamp = robot::Time::now();
+goal.target_pose.pose.position.x = 5.0;
+goal.target_pose.pose.position.y = 3.0;
+goal.target_pose.pose.position.z = 0.0;
+
+// Set orientation
+tf3::Quaternion q;
+q.setRPY(0, 0, 0.5); // yaw = 0.5 radians
+goal.target_pose.pose.orientation = tf3::toMsg(q);
+
+goal.xy_goal_tolerance = 0.2;
+goal.yaw_goal_tolerance = 0.1;
+goal.goal_id = "goal_1";
+
+if (action_server.sendGoal(goal)) {
+    std::cout << "Goal accepted!" << std::endl;
+} else {
+    std::cout << "Goal rejected!" << std::endl;
+}
+```
+
+### 4. Hủy Goal
+
+```cpp
+if (action_server.cancelGoal()) {
+    std::cout << "Goal cancelled" << std::endl;
+}
+```
+
+### 5. Kiểm tra trạng thái
+
+```cpp
+// Kiểm tra có goal mới không (cho preemption)
+if (action_server.isNewGoalAvailable()) {
+    move_base::MoveBaseActionGoal new_goal = action_server.acceptNewGoal();
+    // Xử lý goal mới
+}
+
+// Kiểm tra có yêu cầu preempt không
+if (action_server.isPreemptRequested()) {
+    // Dừng và cleanup
+}
+```
+
+## Ví dụ đầy đủ
+
+Xem file `examples/action_server_example.cpp` để có ví dụ hoàn chỉnh.
+
+## Thread Safety
+
+- `MoveBaseActionServer` sử dụng mutex để đảm bảo thread-safe
+- Control loop chạy trong thread riêng
+- Tất cả các method đều thread-safe
+
+## Lưu ý
+
+1. **Control Loop**: Action Server tự động chạy control loop ở tần số đã chỉ định (mặc định 20 Hz)
+2. **Goal Management**: Chỉ có thể có 1 goal active tại một thời điểm. Goal mới sẽ preempt goal cũ.
+3. **Feedback**: Feedback được gửi tự động qua callback, không cần gọi `publishFeedback()` như ROS
+4. **Result**: Result được set tự động dựa trên state của MoveBase, hoặc có thể set thủ công
+
+## Migration từ ROS Action Server
+
+Nếu bạn đang dùng ROS Action Server, migration rất đơn giản:
+
+**Trước (ROS):**
+```cpp
+MoveBaseActionServer *as_;
+as_ = new MoveBaseActionServer(ros::NodeHandle(), "move_base", 
+    [this](auto &goal) { executeCb(goal); }, false);
+as_->start();
+```
+
+**Sau (MoveBaseActionServer):**
+```cpp
+move_base::MoveBaseActionServer action_server(move_base_ptr, 20.0);
+action_server.setGoalCallback([this](const auto& goal) { 
+    executeCb(goal); 
+});
+action_server.start();
+```
+
+## API Reference
+
+Xem file `include/move_base/move_base_action_server.h` để có đầy đủ API documentation.
+
--- a/src/Navigations/Packages/move_base/examples/action_server_example.cpp
+++ b/src/Navigations/Packages/move_base/examples/action_server_example.cpp
@@ -0,0 +1,192 @@
+/*********************************************************************
+ *
+ * Example: Using MoveBaseActionServer
+ * 
+ * This example demonstrates how to use MoveBaseActionServer
+ * which provides a ROS Action Server-like interface
+ *********************************************************************/
+
+#include <move_base/move_base_action_server.h>
+#include <move_base/move_base.h>
+#include <robot/node_handle.h>
+#include <robot/console.h>
+#include <robot/time.h>
+#include <tf3/buffer_core.h>
+#include <robot/plugin_loader_helper.h>
+#include <boost/dll/import.hpp>
+#include <iostream>
+#include <thread>
+#include <chrono>
+
+using namespace move_base;
+
+void goalCallback(const MoveBaseActionGoal& goal)
+{
+  robot::log_info("[Example] New goal received:");
+  robot::log_info("  Position: (%.2f, %.2f)", 
+                  goal.target_pose.pose.position.x,
+                  goal.target_pose.pose.position.y);
+  robot::log_info("  Frame: %s", goal.target_pose.header.frame_id.c_str());
+  robot::log_info("  XY Tolerance: %.2f", goal.xy_goal_tolerance);
+  robot::log_info("  Yaw Tolerance: %.2f", goal.yaw_goal_tolerance);
+}
+
+void feedbackCallback(const MoveBaseActionFeedback& feedback)
+{
+  const auto& nav_fb = feedback.nav_feedback;
+  robot::log_info("[Example] Feedback:");
+  robot::log_info("  State: %s", 
+                  robot::move_base_core::toString(nav_fb.navigation_state).c_str());
+  robot::log_info("  Current Pose: (%.2f, %.2f, %.2f)",
+                  nav_fb.current_pose.x,
+                  nav_fb.current_pose.y,
+                  nav_fb.current_pose.theta);
+  robot::log_info("  Distance to Goal: %.2f m", feedback.distance_to_goal);
+  if (!nav_fb.feed_back_str.empty())
+  {
+    robot::log_info("  Message: %s", nav_fb.feed_back_str.c_str());
+  }
+}
+
+void resultCallback(const MoveBaseActionResult& result)
+{
+  robot::log_info("[Example] Result received:");
+  robot::log_info("  Success: %s", result.success ? "Yes" : "No");
+  robot::log_info("  Final State: %s",
+                  robot::move_base_core::toString(result.final_state).c_str());
+  robot::log_info("  Message: %s", result.message.c_str());
+}
+
+void preemptCallback()
+{
+  robot::log_warning("[Example] Goal was preempted!");
+}
+
+int main(int argc, char** argv)
+{
+  try
+  {
+    robot::log_info("[Example] Starting MoveBaseActionServer example...");
+
+    // 1. Create TF buffer
+    std::shared_ptr<tf3::BufferCore> tf = std::make_shared<tf3::BufferCore>();
+    
+    // Set up static transforms (example)
+    // In real usage, these would come from your localization system
+    // tf->setTransform(...);
+
+    // 2. Load and initialize MoveBase
+    robot::log_info("[Example] Loading MoveBase plugin...");
+    robot::PluginLoaderHelper loader;
+    std::string path_file_so = loader.findLibraryPath("MoveBase");
+    auto create_plugin = boost::dll::import_alias<robot::move_base_core::BaseNavigation::Ptr()>(
+        path_file_so,
+        "MoveBase", 
+        boost::dll::load_mode::append_decorations);
+    
+    robot::move_base_core::BaseNavigation::Ptr move_base_ptr = create_plugin();
+    if (!move_base_ptr)
+    {
+      robot::log_error("[Example] Failed to create MoveBase instance");
+      return 1;
+    }
+
+    robot::log_info("[Example] Initializing MoveBase...");
+    move_base_ptr->initialize(tf);
+    
+    // Wait for initialization
+    robot::Rate init_rate(10);
+    while (!move_base_ptr->getFeedback()->is_ready)
+    {
+      init_rate.sleep();
+    }
+    robot::log_info("[Example] MoveBase is ready!");
+
+    // 3. Create Action Server
+    robot::log_info("[Example] Creating MoveBaseActionServer...");
+    MoveBaseActionServer action_server(move_base_ptr, 20.0); // 20 Hz control loop
+
+    // 4. Set up callbacks
+    action_server.setGoalCallback(goalCallback);
+    action_server.setFeedbackCallback(feedbackCallback);
+    action_server.setResultCallback(resultCallback);
+    action_server.setPreemptCallback(preemptCallback);
+
+    // 5. Start the action server
+    action_server.start();
+    robot::log_info("[Example] Action server started");
+
+    // 6. Example: Send a goal
+    std::this_thread::sleep_for(std::chrono::seconds(1));
+    
+    MoveBaseActionGoal goal;
+    goal.target_pose.header.frame_id = "map";
+    goal.target_pose.header.stamp = robot::Time::now();
+    goal.target_pose.pose.position.x = 5.0;
+    goal.target_pose.pose.position.y = 3.0;
+    goal.target_pose.pose.position.z = 0.0;
+    
+    // Set orientation (yaw = 0.5 radians)
+    tf3::Quaternion q;
+    q.setRPY(0, 0, 0.5);
+    goal.target_pose.pose.orientation = tf3::toMsg(q);
+    
+    goal.xy_goal_tolerance = 0.2;
+    goal.yaw_goal_tolerance = 0.1;
+    goal.goal_id = "example_goal_1";
+
+    robot::log_info("[Example] Sending goal...");
+    if (action_server.sendGoal(goal))
+    {
+      robot::log_info("[Example] Goal accepted!");
+    }
+    else
+    {
+      robot::log_error("[Example] Goal rejected!");
+      return 1;
+    }
+
+    // 7. Monitor progress (in real application, this would be in your main loop)
+    robot::log_info("[Example] Monitoring navigation progress...");
+    robot::Rate monitor_rate(1.0); // Check every second
+    
+    int timeout_seconds = 60;
+    int elapsed_seconds = 0;
+    
+    while (action_server.isRunning() && elapsed_seconds < timeout_seconds)
+    {
+      // Check if goal is still active
+      if (!action_server.isNewGoalAvailable() && !action_server.isPreemptRequested())
+      {
+        // Goal is being processed
+        robot::log_info("[Example] Navigation in progress... (elapsed: %d s)", elapsed_seconds);
+      }
+      
+      monitor_rate.sleep();
+      elapsed_seconds++;
+    }
+
+    // 8. Example: Cancel goal (optional)
+    // Uncomment to test cancellation:
+    // robot::log_info("[Example] Cancelling goal...");
+    // action_server.cancelGoal();
+
+    // 9. Cleanup
+    robot::log_info("[Example] Stopping action server...");
+    action_server.stop();
+    
+    robot::log_info("[Example] Example completed successfully");
+    return 0;
+  }
+  catch (const std::exception& e)
+  {
+    robot::log_error("[Example] Exception: %s", e.what());
+    return 1;
+  }
+  catch (...)
+  {
+    robot::log_error("[Example] Unknown exception occurred");
+    return 1;
+  }
+}
+
--- a/src/Navigations/Packages/move_base/include/move_base/convert_data.h
+++ b/src/Navigations/Packages/move_base/include/move_base/convert_data.h
@@ -0,0 +1,37 @@
+#ifndef MOVE_BASE_CONVERT_DATA_H
+#define MOVE_BASE_CONVERT_DATA_H
+
+#include <robot_costmap_2d/costmap_2d_robot.h>
+#include <robot_nav_msgs/OccupancyGrid.h>
+#include <robot_map_msgs/OccupancyGridUpdate.h>
+
+namespace move_base
+{
+  class ConvertData
+  {
+  public:
+    ConvertData(robot_costmap_2d::Costmap2DROBOT* costmap, std::string global_frame, bool always_send_full_costmap = false);
+    ~ConvertData();
+
+    /** @brief Update data for publication. */
+    void updateCostmap(robot_nav_msgs::OccupancyGrid &grid, robot_map_msgs::OccupancyGridUpdate &update, bool &is_updated);
+
+    void updateFootprint(robot_geometry_msgs::PolygonStamped &footprint);
+
+  private:
+    /** @brief Prepare grid_ message for publication. */
+    void prepareGrid();
+
+    /** @brief Include the given bounds in the changed-rectangle. */
+    void updateBounds(unsigned int x0, unsigned int xn, unsigned int y0, unsigned int yn);
+
+    robot_costmap_2d::Costmap2DROBOT* costmap_;
+    std::string global_frame_;
+    unsigned int x0_, xn_, y0_, yn_;
+    double saved_origin_x_, saved_origin_y_;
+    bool always_send_full_costmap_;
+    robot_nav_msgs::OccupancyGrid grid_;
+    static char* cost_translation_table_;  ///< Translate from 0-255 values in costmap to -1 to 100 values in message.
+  };
+}
+#endif
--- a/src/Navigations/Packages/move_base/include/move_base/move_base.h
+++ b/src/Navigations/Packages/move_base/include/move_base/move_base.h
@@ -31,6 +31,7 @@

 #include <robot/node_handle.h>
 #include <robot/console.h>
+#include <robot/timer.h>

 namespace move_base
 {
@@ -75,6 +76,9 @@ namespace move_base
     */
    virtual void initialize(robot::TFListenerPtr tf) override;

+
+    static void initializeCostToOccupancyLUT();
+
    /**
     * @brief Set the robot's footprint (outline shape) in the global frame.
     *        This can be used for planning or collision checking.
@@ -350,10 +354,9 @@ namespace move_base

    /**
     * @brief Get the robot’s twist.
-     * @param twist Output parameter with the robot’s current twist.
-     * @return True if twist was successfully retrieved.
+     * @return The robot’s current twist.
     */
-    virtual bool getTwist(robot_nav_2d_msgs::Twist2DStamped &twist) override;
+    virtual robot_nav_2d_msgs::Twist2DStamped getTwist() override;

    /**
     * @brief Get the navigation feedback.
@@ -361,6 +364,17 @@ namespace move_base
     */
    virtual robot::move_base_core::NavFeedback *getFeedback() override;

+    /**
+     * @brief Get the global data.
+     * @return The global data.
+     */
+    virtual robot::move_base_core::PlannerDataOutput getGlobalData() override;
+
+    /**
+     * @brief Get the local data.
+     * @return The local data.
+     */
+    virtual robot::move_base_core::PlannerDataOutput getLocalData() override;

    /**
     * @brief  Destructor - Cleans up
@@ -378,18 +392,20 @@ namespace move_base
    /**
    * @brief Update the global costmap.
    * @param value The value to update the costmap.
+    * @param layer_type The type of the layer.
    * @param name The name of the costmap.
    */
    template<typename T>
-    void updateGlobalCostmap(const T& value, const std::string &name);
+    void updateGlobalCostmap(const T& value, robot_costmap_2d::LayerType layer_type, const std::string &name);

    /**
    * @brief Update the local costmap.
    * @param value The value to update the costmap.
+    * @param layer_type The type of the layer.
    * @param name The name of the costmap.
    */
    template<typename T>
-    void updateLocalCostmap(const T& value, const std::string &name);
+    void updateLocalCostmap(const T& value, robot_costmap_2d::LayerType layer_type, const std::string &name);
    
    /**
     * @brief  Performs a control cycle
@@ -466,10 +482,10 @@ namespace move_base

    robot_geometry_msgs::PoseStamped goalToGlobalFrame(const robot_geometry_msgs::PoseStamped &goal_pose_msg);

-    // /**
-    //  * @brief This is used to wake the planner at periodic intervals.
-    //  */
-    // void wakePlanner(const robot::TimerEvent &event);
+    /**
+     * @brief This is used to wake the planner at periodic intervals.
+     */
+    void wakePlanner(const robot::TimerEvent &event);

  private:
    bool initialized_;
@@ -497,9 +513,6 @@ namespace move_base
    int32_t max_planning_retries_;
    uint32_t planning_retries_;
    double conservative_reset_dist_, clearing_radius_;
-    // robot::Publisher current_goal_pub_, vel_pub_, action_goal_pub_, recovery_status_pub_;
-    // robot::Subscriber goal_sub_;
-    // robot::ServiceServer make_plan_srv_, clear_costmaps_srv_;
    bool shutdown_costmaps_, clearing_rotation_allowed_;
    bool make_plan_clear_costmap_, make_plan_add_unreachable_goal_;
    double oscillation_timeout_, oscillation_distance_;
@@ -522,6 +535,7 @@ namespace move_base
    robot_geometry_msgs::PoseStamped planner_goal_;
    boost::thread *planner_thread_;

+    boost::recursive_mutex configuration_mutex_;
    move_base::MoveBaseConfig last_config_;
    move_base::MoveBaseConfig default_config_;

@@ -539,6 +553,7 @@ namespace move_base
    std::string docking_planner_name_{"NAVDockingLocalPlanner"};
    std::string go_straight_planner_name_{"NAVGoStraightLocalPlanner"};
    std::string rotate_planner_name_{"NAVRotateLocalPlanner"};
+  
  };

 } // namespace move_base
--- a/src/Navigations/Packages/move_base/src/convert_data.cpp
+++ b/src/Navigations/Packages/move_base/src/convert_data.cpp
@@ -0,0 +1,131 @@
+#include <move_base/convert_data.h>
+#include <robot/time.h>
+#include <robot/console.h>
+#include <robot_costmap_2d/cost_values.h>
+
+char* move_base::ConvertData::cost_translation_table_ = NULL;
+
+move_base::ConvertData::ConvertData(robot_costmap_2d::Costmap2DROBOT* costmap, std::string global_frame, bool always_send_full_costmap)
+  : costmap_(costmap), global_frame_(global_frame), always_send_full_costmap_(always_send_full_costmap)
+{
+    if (cost_translation_table_ == NULL)
+    {
+      cost_translation_table_ = new char[256];
+  
+      // special values:
+      cost_translation_table_[(const int)robot_costmap_2d::PREFERRED_SPACE] = 120;  // Preferred place
+      for(int i = (int)robot_costmap_2d::PREFERRED_SPACE+1; i <= (const int)robot_costmap_2d::FREE_SPACE; i++)
+        cost_translation_table_[i] = 0;   // NO obstacle
+  
+      cost_translation_table_[(const int)robot_costmap_2d::CRITICAL_SPACE] = 150;  // CRITICAL_SPACE
+      cost_translation_table_[(const int)robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE] = 99;  // INSCRIBED obstacle
+      cost_translation_table_[(const int)robot_costmap_2d::LETHAL_OBSTACLE] = 100;  // LETHAL obstacle
+      cost_translation_table_[(const int)robot_costmap_2d::NO_INFORMATION] = -1;  // UNKNOWN
+  
+      // regular cost values scale the range 10 to 252 (inclusive) to fit
+      // into 1 to 98 (inclusive).
+      for (int i = (int)robot_costmap_2d::FREE_SPACE + 1; i < (const int)robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE; i++)
+      {
+        cost_translation_table_[ i ] = char(1 + (97 * (i - 1)) / 251);
+      }
+    }
+
+    xn_ = yn_ = 0;
+    x0_ = costmap_->getCostmap()->getSizeInCellsX();
+    y0_ = costmap_->getCostmap()->getSizeInCellsY();
+}
+
+move_base::ConvertData::~ConvertData()
+{
+}
+
+// prepare grid_ message for publication.
+void move_base::ConvertData::prepareGrid()
+{
+  // boost::unique_lock<Costmap2D::mutex_t> lock(*(costmap_->getMutex()));
+  double resolution = costmap_->getCostmap()->getResolution();
+
+  grid_.header.frame_id = global_frame_;
+  grid_.header.stamp = robot::Time::now();
+  grid_.info.resolution = resolution;
+
+  grid_.info.width = costmap_->getCostmap()->getSizeInCellsX();
+  grid_.info.height = costmap_->getCostmap()->getSizeInCellsY();
+
+  double wx, wy;
+  costmap_->getCostmap()->mapToWorld(0, 0, wx, wy);
+  grid_.info.origin.position.x = wx - resolution / 2;
+  grid_.info.origin.position.y = wy - resolution / 2;
+  grid_.info.origin.position.z = 0.0;
+  grid_.info.origin.orientation.w = 1.0;
+  saved_origin_x_ = costmap_->getCostmap()->getOriginX();
+  saved_origin_y_ = costmap_->getCostmap()->getOriginY();
+
+  grid_.data.resize(grid_.info.width * grid_.info.height);
+
+  unsigned char* data = costmap_->getCostmap()->getCharMap();
+  for (unsigned int i = 0; i < grid_.data.size(); i++)
+  {
+    grid_.data[i] = cost_translation_table_[ data[ i ]];
+  }
+}
+
+void move_base::ConvertData::updateCostmap(robot_nav_msgs::OccupancyGrid &grid, robot_map_msgs::OccupancyGridUpdate &update, bool &is_updated)
+{
+  boost::unique_lock<robot_costmap_2d::Costmap2D::mutex_t> lock(*(costmap_->getCostmap()->getMutex()));
+  float resolution = costmap_->getCostmap()->getResolution();
+  unsigned int x0, y0, xn, yn;
+  costmap_->getLayeredCostmap()->getBounds(&x0, &xn, &y0, &yn);
+  updateBounds(x0, xn, y0, yn);
+  if (always_send_full_costmap_ || grid_.info.resolution != resolution ||
+      grid_.info.width != costmap_->getCostmap()->getSizeInCellsX() ||
+      grid_.info.height != costmap_->getCostmap()->getSizeInCellsY() ||
+      saved_origin_x_ != costmap_->getCostmap()->getOriginX() ||
+      saved_origin_y_ != costmap_->getCostmap()->getOriginY())
+  {
+    prepareGrid();
+    grid = grid_;
+    is_updated = false;
+  }
+  else if (x0_ < xn_)
+  {
+    update.header.stamp = robot::Time::now();
+    update.header.frame_id = global_frame_;
+    update.x = x0_;
+    update.y = y0_;
+    update.width = xn_ - x0_;
+    update.height = yn_ - y0_;
+    update.data.resize(update.width * update.height);
+
+    unsigned int i = 0;
+    for (unsigned int y = y0_; y < yn_; y++)
+    {
+      for (unsigned int x = x0_; x < xn_; x++)
+      {
+        unsigned char cost = costmap_->getCostmap()->getCost(x, y);
+        update.data[i++] = cost_translation_table_[ cost ];
+      }
+    } 
+    is_updated = true;
+  }
+
+  xn_ = yn_ = 0;
+  x0_ = costmap_->getCostmap()->getSizeInCellsX();
+  y0_ = costmap_->getCostmap()->getSizeInCellsY();
+}
+
+void move_base::ConvertData::updateFootprint(robot_geometry_msgs::PolygonStamped &footprint)
+{
+  footprint.header.frame_id = global_frame_;
+  footprint.header.stamp = robot::Time::now();
+  footprint.polygon = costmap_->getRobotFootprintPolygonStamped().polygon;
+
+}
+
+void move_base::ConvertData::updateBounds(unsigned int x0, unsigned int xn, unsigned int y0, unsigned int yn)
+{
+  x0_ = std::min(x0, x0_);
+  xn_ = std::max(xn, xn_);
+  y0_ = std::min(y0, y0_);
+  yn_ = std::max(yn, yn_);
+}
--- a/src/Navigations/Packages/move_base/src/move_base.cpp
+++ b/src/Navigations/Packages/move_base/src/move_base.cpp