update

2026-01-12 15:49:25 +07:00 · 2026-01-12 15:49:25 +07:00 · 145fb2088e
commit 145fb2088e
parent f5e7e1f1e0
29 changed files with 3091 additions and 771 deletions
--- a/config/config/costmap_params.yaml
+++ b/config/config/costmap_params.yaml
@ -0,0 +1,36 @@
+robot_costmap_2d:
+  global_frame: map
+  robot_base_frame: base_link
+  rolling_window: false
+  track_unknown_space: false
+
+  plugins:
+    - name: static_layer
+      type: StaticLayer
+
+    - name: inflation_layer
+      type: InflationLayer
+
+    - name: obstacle_layer
+      type: ObstacleLayer
+
+    - name: voxel_layer
+      type: VoxelLayer
+
+  library_path: ./libplugins.so
+  footprint:
+    - [0.3, 0.3]
+    - [0.3, -0.3]
+    - [-0.3, -0.3]
+    - [-0.3, 0.3]
+
+  transform_tolerance: 0.0
+  update_frequency: 1.0
+  width: 0.0
+  height: 0.0
+  resolution: 0.0
+  origin_x: 0.0
+  origin_y: 0.0
+
+  footprint_padding: 0.0
+  robot_radius: 0.0
--- a/config/config/inflation_layer_params.yaml
+++ b/config/config/inflation_layer_params.yaml
@ -0,0 +1,5 @@
+inflation_layer:
+  enabled: true
+  inflate_unknown: false
+  cost_scaling_factor: 15.0
+  inflation_radius: 0.55
--- a/config/config/obstacle_layer_params.yaml
+++ b/config/config/obstacle_layer_params.yaml
@ -0,0 +1,18 @@
+obstacle_layer:
+  track_unknown_space: true
+  transform_tolerance: 0.2
+  topic: "map"
+  sensor_frame: laser_frame
+  observation_persistence: 0.0
+  expected_update_rate: 0.0
+  data_type: PointCloud
+  min_obstacle_height: 0.0
+  max_obstacle_height: 2.0
+  inf_is_valid: false
+  clearing: false
+  marking: true
+  obstacle_range: 2.5
+  raytrace_range: 3.0
+  footprint_clearing_enabled: true
+  combination_method: 1
+
--- a/config/config/static_layer_params.yaml
+++ b/config/config/static_layer_params.yaml
@ -0,0 +1,11 @@
+static_layer:
+  enabled: true
+  map_topic: "map"
+  first_map_only: false
+  subscribe_to_updates: false
+  track_unknown_space: true
+  use_maximum: false
+  lethal_cost_threshold: 100
+  unknown_cost_value: -1
+  trinary_costmap: true
+  base_frame_id: "map"
--- a/config/config/voxel_layer_params.yaml
+++ b/config/config/voxel_layer_params.yaml
@ -0,0 +1,11 @@
+voxel_layer:
+  enabled: true
+  footprint_clearing_enabled: true
+  max_obstacle_height: 3.0
+  origin_z: 0.0
+  z_resolution: 0.2
+  z_voxels: 16
+  unknown_threshold: 15.0
+  mark_threshold: 0
+  combination_method: 1
+
--- a/config/costmap_common_params.yaml
+++ b/config/costmap_common_params.yaml
@ -52,3 +52,115 @@ obstacles:
    inf_is_valid: false
    min_obstacle_height: 0.0
    max_obstacle_height: 0.25
+
+
+global_costmap:
+  robot_base_frame: base_footprint
+  transform_tolerance: 1.0
+  obstacle_range: 3.0
+  #mark_threshold: 1
+  publish_voxel_map: true
+  footprint_padding: 0.0
+
+  navigation_map:
+    map_topic: /map
+    map_pkg: managerments
+    map_file: maze
+
+  virtual_walls_map:
+    map_topic: /virtual_walls/map
+    namespace: /virtual_walls
+    map_pkg: managerments
+    map_file: maze
+    use_maximum: true
+    lethal_cost_threshold: 100
+
+  obstacles:
+    observation_sources: ffffff_scan_marking f_scan_clearing b_scan_marking b_scan_clearing
+    ffffff_scan_marking:
+      topic: /f_scan
+      data_type: LaserScan
+      clearing: false
+      marking: true
+      inf_is_valid: false
+      min_obstacle_height: 0.0
+      max_obstacle_height: 0.25
+    f_scan_clearing:
+      topic: /f_scan
+      data_type: LaserScan
+      clearing: true
+      marking: false
+      inf_is_valid: false
+      min_obstacle_height: 0.0
+      max_obstacle_height: 0.25
+    b_scan_marking:
+      topic: /b_scan
+      data_type: LaserScan
+      clearing: false
+      marking: true
+      inf_is_valid: false
+      min_obstacle_height: 0.0
+      max_obstacle_height: 0.25
+    b_scan_clearing:
+      topic: /b_scan
+      data_type: LaserScan
+      clearing: true
+      marking: false
+      inf_is_valid: false
+      min_obstacle_height: 0.0
+      max_obstacle_height: 0.25
+local_costmap:
+  robot_base_frame: base_footprint
+  transform_tolerance: 1.0
+  obstacle_range: 3.0
+  #mark_threshold: 1
+  publish_voxel_map: true
+  footprint_padding: 0.0
+
+  navigation_map:
+    map_topic: /map
+    map_pkg: managerments
+    map_file: maze
+
+  virtual_walls_map:
+    map_topic: /virtual_walls/map
+    namespace: /virtual_walls
+    map_pkg: managerments
+    map_file: maze
+    use_maximum: true
+    lethal_cost_threshold: 100
+
+  obstacles:
+    observation_sources: ffffff_scan_marking f_scan_clearing b_scan_marking b_scan_clearing
+    ffffff_scan_marking:
+      topic: /f_scan
+      data_type: LaserScan
+      clearing: false
+      marking: true
+      inf_is_valid: false
+      min_obstacle_height: 0.0
+      max_obstacle_height: 0.25
+    f_scan_clearing:
+      topic: /f_scan
+      data_type: LaserScan
+      clearing: true
+      marking: false
+      inf_is_valid: false
+      min_obstacle_height: 0.0
+      max_obstacle_height: 0.25
+    b_scan_marking:
+      topic: /b_scan
+      data_type: LaserScan
+      clearing: false
+      marking: true
+      inf_is_valid: false
+      min_obstacle_height: 0.0
+      max_obstacle_height: 0.25
+    b_scan_clearing:
+      topic: /b_scan
+      data_type: LaserScan
+      clearing: true
+      marking: false
+      inf_is_valid: false
+      min_obstacle_height: 0.0
+      max_obstacle_height: 0.25
--- a/doc/architecture_discussion.md
+++ b/doc/architecture_discussion.md
@ -205,20 +205,64 @@ Cần làm rõ:
   - Thực thi control loop (executeCycle)

 4. **Planning Layer** ✅
-   - `nav_core::BaseGlobalPlanner`: Interface cho global planners (A*, D*, etc.)
-   - `nav_core::BaseLocalPlanner`: Interface cho local planners (DWA, TEB, MKT, etc.)
-   - `nav_core::RecoveryBehavior`: Interface cho recovery behaviors
-   - Plugin system sử dụng `boost::dll` để dynamic loading
+   - `robot_nav_core::BaseGlobalPlanner`: Interface cho global planners
+   - **Global Planner Implementations**:
+     - `custom_planner` - Custom path planner với curve support và merge path calculation
+     - `dock_planner` - Docking-specific planner với dock calculation utilities
+     - `two_points_planner` - Simple planner cho two-point navigation
+   - `robot_nav_core::BaseLocalPlanner`: Interface cho local planners
+   - **Local Planner Implementation**:
+     - `pnkx_local_planner` - Main local planner với multiple behaviors:
+       - Position planning (MKT algorithm)
+       - Docking planning
+       - Go straight planning
+       - Rotate planning
+   - `robot_nav_core::RecoveryBehavior`: Interface cho recovery behaviors
+   - Plugin system sử dụng `boost::dll` và `robot::PluginLoaderHelper` để dynamic loading

 5. **Costmap Layer** ✅
-   - `robot_costmap_2d::Costmap2DROBOT`: Global và local costmap
-   - Costmap layers: static map, obstacles, inflation
+   - `robot_costmap_2d::Costmap2DROBOT`: Global và local costmap wrapper
+   - `robot_costmap_2d::LayeredCostmap`: Layered costmap system
+   - **Costmap Layers** (8 implementations):
+     - `StaticLayer` - Static map từ OccupancyGrid
+     - `ObstacleLayer` - Dynamic obstacles từ sensors
+     - `InflationLayer` - Cost inflation cho robot footprint
+     - `VoxelLayer` - 3D voxel grid cho obstacles
+     - `CriticalLayer` - Critical zones marking
+     - `DirectionalLayer` - Directional cost preferences
+     - `PreferredLayer` - Preferred path zones
+     - `UnpreferredLayer` - Unpreferred zones
   - Frame management: map (global), odom (local)
+   - Thread-safe access với mutex
+   - Observation buffer cho sensor data

 6. **Algorithms Layer** ✅
-   - `mkt_algorithm`: Diff drive và bicycle kinematics algorithms
-   - `score_algorithm`: Trajectory scoring và goal checking
-   - `kalman`: Filtering algorithms
+   - **MKT Algorithm** (`mkt_algorithm`):
+     - Differential drive kinematics:
+       - `diff_go_straight` - Go straight behavior
+       - `diff_predictive_trajectory` - Predictive trajectory following
+       - `diff_rotate_to_goal` - Rotate to goal
+     - Bicycle model kinematics:
+       - `bicycle_go_straight` - Go straight với bicycle model
+       - `bicycle_pure_pursuit` - Pure pursuit control
+       - `bicycle_rotate_to_goal` - Rotate behavior
+   - **MKT Plugins** (`mkt_plugins`):
+     - `goal_checker` - Goal checking algorithms
+     - `simple_goal_checker` - Simple goal checking
+     - `standard_traj_generator` - Standard trajectory generation
+     - `limited_accel_generator` - Acceleration-limited trajectory generation
+     - `kinematic_parameters` - Kinematics parameter management
+     - `xy_theta_iterator` - Trajectory iteration utilities
+     - `velocity_iterator` - Velocity space iteration
+     - `one_d_velocity_iterator` - 1D velocity iteration
+   - **Score Algorithm** (`score_algorithm`):
+     - Trajectory scoring và evaluation
+     - Goal checking utilities
+     - Trajectory generator interface
+   - **Kalman Filter** (`kalman`):
+     - State estimation
+     - Sensor fusion
+     - Noise filtering

 7. **Data Sources** ⚠️ (Interface cần định nghĩa)
   - Localization source (Pnkx Loc)
@ -250,19 +294,23 @@ Cần làm rõ:
  - `pause()`, `resume()`, `cancel()` - Điều khiển trạng thái
  - `getRobotPose(...)` - Lấy vị trí robot

- `nav_core::BaseGlobalPlanner` ✅
+- `robot_nav_core::BaseGlobalPlanner` ✅
  - `makePlan(start, goal, plan)` - Tạo global path
  - `initialize(name, costmap_robot)` - Khởi tạo với costmap
+  - **Implementations**: `custom_planner`, `dock_planner`, `two_points_planner`

- `nav_core::BaseLocalPlanner` ✅
-  - `computeVelocityCommands(cmd_vel)` - Tính toán velocity command
+- `robot_nav_core::BaseLocalPlanner` ✅
+  - `computeVelocityCommands(odom, cmd_vel)` - Tính toán velocity command
  - `setPlan(plan)` - Set global path để follow
  - `isGoalReached()` - Kiểm tra đã đến goal chưa
-  - `swapPlanner(name)` - Thay đổi planner động
+  - `swapPlanner(name)` - Thay đổi planner động (position, docking, go_straight, rotate)
  - `setTwistLinear/Angular(...)` - Set velocity limits
+  - `getTwistLinear/Angular(...)` - Get velocity limits
+  - **Implementation**: `pnkx_local_planner` với multiple behaviors

- `nav_core::RecoveryBehavior` ✅
+- `robot_nav_core::RecoveryBehavior` ✅
  - `runBehavior()` - Thực thi recovery behavior
+  - `initialize(name, tf, global_costmap, local_costmap)` - Khởi tạo

 - `robot_costmap_2d::Costmap2DROBOT` ✅
  - Wrapper cho costmap với robot footprint
@ -290,9 +338,15 @@ Cần làm rõ:

 **Plugin mechanism:** ✅
 - Sử dụng `boost::dll` để dynamic loading plugins
- Factory pattern với `boost::function` và `boost::dll::import`
+- Factory pattern với `boost::function` và `boost::dll::import_alias`
+- `robot::PluginLoaderHelper` để tìm library path từ plugin name
 - Config file YAML để specify plugin names
- Plugin interfaces: `BaseGlobalPlanner`, `BaseLocalPlanner`, `RecoveryBehavior`
+- Plugin interfaces: 
+  - `robot_nav_core::BaseGlobalPlanner` - Global planners
+  - `robot_nav_core::BaseLocalPlanner` - Local planners  
+  - `robot_nav_core::RecoveryBehavior` - Recovery behaviors
+  - `robot_costmap_2d::Layer` - Costmap layers
+- Plugin registration với `BOOST_DLL_ALIAS` macro

 ### 4. Cơ chế giao tiếp & đồng bộ

@ -340,36 +394,99 @@ Cần làm rõ:

 **Đã hoàn thành:** ✅

-1. ✅ **Interface Layer**: `BaseNavigation` interface
-2. ✅ **Implementation Layer**: `MoveBase` core logic
-3. ✅ **Planning Layer**: Plugin system cho global/local planners và recovery
-4. ✅ **Costmap Layer**: Global và local costmap với layers
-5. ✅ **Algorithms Layer**: MKT algorithms, score algorithm, kalman
-6. ✅ **API Layer**: C API wrapper cho .NET integration
-7. ✅ **Supporting Libraries**: tf3, robot_time, geometry_msgs, robot_nav_2d_utils
+1. ✅ **Interface Layer**: 
+   - `move_base_core::BaseNavigation` - Abstract interface hoàn chỉnh
+   - `move_base_core::common.h` - Common types và utilities
+
+2. ✅ **Implementation Layer**: 
+   - `move_base::MoveBase` - Core implementation với state machine
+   - `executeCycle()` - Control loop implementation
+   - State management (PLANNING, CONTROLLING, CLEARING, PAUSED)
+   - Threading với planner thread và control loop
+
+3. ✅ **Planning Layer - Plugin System**: 
+   - **Global Planners** (3 implementations):
+     - `custom_planner` - Custom path planning với curve support
+     - `dock_planner` - Docking-specific planner
+     - `two_points_planner` - Simple two-point planner
+   - **Local Planners** (1 implementation):
+     - `pnkx_local_planner` - Main local planner với:
+       - `pnkx_docking_local_planner` - Docking behavior
+       - `pnkx_go_straight_local_planner` - Go straight behavior
+       - `pnkx_rotate_local_planner` - Rotate behavior
+   - **Recovery Behaviors**: Interface đã định nghĩa
+   - Plugin loading với `boost::dll` và `robot::PluginLoaderHelper`
+
+4. ✅ **Costmap Layer**: 
+   - `robot_costmap_2d::Costmap2DROBOT` - Global và local costmap
+   - **Costmap Layers** (8 implementations):
+     - `static_layer` - Static map layer
+     - `obstacle_layer` - Dynamic obstacles
+     - `inflation_layer` - Cost inflation
+     - `voxel_layer` - 3D voxel grid
+     - `critical_layer` - Critical zones
+     - `directional_layer` - Directional costs
+     - `preferred_layer` - Preferred paths
+     - `unpreferred_layer` - Unpreferred zones
+   - Layered costmap system với plugin support
+
+5. ✅ **Algorithms Layer**: 
+   - **MKT Algorithm**:
+     - Differential drive kinematics (go_straight, rotate, predictive)
+     - Bicycle model kinematics (go_straight, rotate, pure pursuit)
+   - **MKT Plugins**:
+     - `goal_checker` - Goal checking algorithms
+     - `standard_traj_generator` - Standard trajectory generation
+     - `kinematic_parameters` - Kinematics parameter management
+     - `xy_theta_iterator` - Trajectory iteration
+     - `limited_accel_generator` - Acceleration-limited trajectories
+   - **Score Algorithm**: Trajectory scoring và goal checking
+   - **Kalman Filter**: State estimation và filtering
+
+6. ✅ **API Layer**: 
+   - `nav_c_api` - C API wrapper cho .NET/C# P/Invoke
+   - Wrapper functions cho BaseNavigation methods
+
+7. ✅ **Supporting Libraries**: 
+   - `tf3` - Transform system (buffer_core, transforms)
+   - `robot_time` - Time management (Time, Duration, Timer, Rate)
+   - `robot_cpp` - Core utilities:
+     - `robot::init()` - Initialization
+     - `robot::NodeHandle` - YAML config loading
+     - `robot::PluginLoaderHelper` - Plugin discovery
+   - `robot_nav_2d_utils` - 2D navigation utilities (conversions, path_ops, polygons, bounds, tf_help)
+   - `robot_nav_2d_msgs` - 2D navigation messages
+   - `geometry_msgs` - Geometry message types (29 types)
+   - `robot_nav_msgs` - Navigation messages (24 types)
+   - `robot_sensor_msgs` - Sensor messages (30 types)
+   - `laser_geometry` - Laser scan geometry utilities
+   - `voxel_grid` - Voxel grid implementation
+   - `data_convert` - Data conversion utilities
+   - `nav_grid` - Navigation grid utilities

 **Đang triển khai / Cần bổ sung:** ⚠️

 1. ⚠️ **Data Sources Interfaces**: 
-   - `ILocalizationSource` interface
-   - `IOdometrySource` interface
-   - `IMapProvider` interface
-   - Integration với Pnkx Loc, odometry sources
+   - `ILocalizationSource` interface - Cần định nghĩa
+   - `IOdometrySource` interface - Cần định nghĩa
+   - `IMapProvider` interface - Cần định nghĩa
+   - Integration với Pnkx Loc, odometry sources - Hiện tại dùng trực tiếp qua MoveBase methods

 2. ⚠️ **Base Controller**:
-   - `IBaseController` interface
-   - Diff drive controller implementation
-   - Steer drive controller implementation
-   - Velocity command execution
+   - `IBaseController` interface - Cần định nghĩa
+   - Diff drive controller implementation - Cần implement
+   - Steer drive controller implementation - Cần implement
+   - Velocity command execution - Hiện tại chỉ có `setTwistLinear/Angular` trong local planner

-3. ⚠️ **Control Loop**:
-   - Control loop trong MoveBase (executeCycle)
-   - State machine management hoàn chỉnh
-   - Threading và synchronization
+3. ⚠️ **User Controller Plugin System**:
+   - Factory để load user controller plugins - Cần implement
+   - Interface cho User Controller - Cần định nghĩa
+   - Integration với BaseNavigation - Cần implement

-4. ⚠️ **User Controller Plugin System**:
-   - Factory để load user controller plugins
-   - Integration với BaseNavigation
+4. ⚠️ **Testing & Documentation**:
+   - Unit tests cho các modules - Một số đã có (costmap, bounds, etc.)
+   - Integration tests - Cần bổ sung
+   - Simulation environment - TODO

 **Lộ trình tiếp theo:**

@ -406,38 +523,144 @@ pnkx_nav_core/
 ├── src/
 │   ├── Navigations/
 │   │   ├── Cores/
-│   │   │   ├── move_base_core/     # BaseNavigation interface
-│   │   │   ├── nav_core/            # Planner interfaces
-│   │   │   ├── nav_core_adapter/    # Adapter utilities
-│   │   │   └── nav_core2/           # Additional nav utilities
+│   │   │   ├── move_base_core/              # BaseNavigation interface ✅
+│   │   │   │   ├── include/move_base_core/
+│   │   │   │   │   ├── navigation.h         # BaseNavigation abstract class
+│   │   │   │   │   └── common.h             # Common types (TFListenerPtr, etc.)
+│   │   │   ├── robot_nav_core/              # Planner interfaces ✅
+│   │   │   │   ├── include/robot_nav_core/
+│   │   │   │   │   ├── base_global_planner.h
+│   │   │   │   │   ├── base_local_planner.h
+│   │   │   │   │   └── recovery_behavior.h
+│   │   │   ├── robot_nav_core_adapter/      # Adapter utilities ✅
+│   │   │   │   ├── include/robot_nav_core_adapter/
+│   │   │   │   │   ├── global_planner_adapter.h
+│   │   │   │   │   ├── local_planner_adapter.h
+│   │   │   │   │   └── costmap_adapter.h
+│   │   │   └── robot_nav_core2/             # Additional nav utilities ✅
+│   │   │       ├── include/robot_nav_core2/
+│   │   │       │   ├── global_planner.h
+│   │   │       │   ├── local_planner.h
+│   │   │       │   └── costmap.h
 │   │   ├── Libraries/
-│   │   │   ├── robot_costmap_2d/          # Costmap system
-│   │   │   ├── tf3/                 # Transform system
-│   │   │   ├── robot_time/          # Time management
-│   │   │   ├── geometry2/           # Geometry utilities
-│   │   │   └── ...                  # Other supporting libraries
+│   │   │   ├── nav_grid/                    # Navigation grid utilities ✅
+│   │   │   └── (costmap, tf3, robot_time ở Libraries/)
 │   │   └── Packages/
-│   │       └── move_base/           # MoveBase implementation
+│   │       └── move_base/                   # MoveBase implementation ✅
+│   │           ├── include/move_base/
+│   │           │   └── move_base.h
+│   │           └── src/
+│   │               ├── move_base.cpp
+│   │               └── move_base_main.cpp
+│   │
 │   ├── Algorithms/
 │   │   ├── Cores/
-│   │   │   └── score_algorithm/     # Trajectory scoring
-│   │   └── Libraries/
-│   │       ├── mkt_algorithm/      # MKT kinematics algorithms
-│   │       ├── kalman/              # Kalman filtering
-│   │       └── angles/               # Angle utilities
+│   │   │   └── score_algorithm/             # Trajectory scoring ✅
+│   │   │       ├── include/score_algorithm/
+│   │   │       │   ├── score_algorithm.h
+│   │   │       │   ├── goal_checker.h
+│   │   │       │   └── trajectory_generator.h
+│   │   ├── Libraries/
+│   │   │   ├── mkt_algorithm/               # MKT kinematics algorithms ✅
+│   │   │   │   ├── include/mkt_algorithm/
+│   │   │   │   │   ├── diff/                # Differential drive
+│   │   │   │   │   │   ├── diff_go_straight.h
+│   │   │   │   │   │   ├── diff_predictive_trajectory.h
+│   │   │   │   │   │   └── diff_rotate_to_goal.h
+│   │   │   │   │   └── bicycle/             # Bicycle model
+│   │   │   │   │       ├── bicycle.h
+│   │   │   │   │       ├── go_straight.h
+│   │   │   │   │       └── rotate_to_goal.h
+│   │   │   ├── mkt_plugins/                 # MKT plugin components ✅
+│   │   │   │   ├── include/mkt_plugins/
+│   │   │   │   │   ├── goal_checker.h
+│   │   │   │   │   ├── standard_traj_generator.h
+│   │   │   │   │   ├── kinematic_parameters.h
+│   │   │   │   │   └── xy_theta_iterator.h
+│   │   │   ├── mkt_msgs/                    # MKT message types ✅
+│   │   │   ├── kalman/                      # Kalman filtering ✅
+│   │   │   └── angles/                      # Angle utilities ✅
+│   │   └── Packages/
+│   │       ├── global_planners/
+│   │       │   ├── custom_planner/           # Custom global planner ✅
+│   │       │   ├── dock_planner/            # Docking planner ✅
+│   │       │   └── two_points_planner/      # Two points planner ✅
+│   │       └── local_planners/
+│   │           └── pnkx_local_planner/      # PNKX local planner ✅
+│   │               ├── include/pnkx_local_planner/
+│   │               │   ├── pnkx_local_planner.h
+│   │               │   ├── pnkx_docking_local_planner.h
+│   │               │   ├── pnkx_go_straight_local_planner.h
+│   │               │   └── pnkx_rotate_local_planner.h
+│   │
+│   ├── Libraries/
+│   │   ├── costmap_2d/                      # Costmap system ✅
+│   │   │   ├── include/robot_costmap_2d/
+│   │   │   │   ├── costmap_2d_robot.h
+│   │   │   │   ├── layered_costmap.h
+│   │   │   │   └── layer.h
+│   │   │   ├── plugins/                     # Costmap layers ✅
+│   │   │   │   ├── static_layer.cpp
+│   │   │   │   ├── obstacle_layer.cpp
+│   │   │   │   ├── inflation_layer.cpp
+│   │   │   │   ├── voxel_layer.cpp
+│   │   │   │   ├── critical_layer.cpp
+│   │   │   │   ├── directional_layer.cpp
+│   │   │   │   ├── preferred_layer.cpp
+│   │   │   │   └── unpreferred_layer.cpp
+│   │   │   └── config/                      # Costmap configs
+│   │   ├── tf3/                             # Transform system ✅
+│   │   ├── robot_time/                      # Time management ✅
+│   │   ├── robot_cpp/                       # Core utilities ✅
+│   │   │   ├── include/robot/
+│   │   │   │   ├── init.h                   # Initialization
+│   │   │   │   ├── node_handle.h            # NodeHandle (YAML config)
+│   │   │   │   └── plugin_loader_helper.h   # Plugin loader
+│   │   ├── robot_nav_2d_utils/              # 2D nav utilities ✅
+│   │   ├── robot_nav_2d_msgs/               # 2D nav messages ✅
+│   │   ├── geometry2/                       # Geometry utilities ✅
+│   │   │   ├── robot_tf3_geometry_msgs/
+│   │   │   └── robot_tf3_sensor_msgs/
+│   │   ├── common_msgs/                     # Message types ✅
+│   │   │   ├── robot_geometry_msgs/
+│   │   │   ├── robot_nav_msgs/
+│   │   │   ├── robot_sensor_msgs/
+│   │   │   ├── robot_std_msgs/
+│   │   │   ├── robot_protocol_msgs/
+│   │   │   └── robot_visualization_msgs/
+│   │   ├── laser_geometry/                  # Laser geometry ✅
+│   │   ├── voxel_grid/                      # Voxel grid ✅
+│   │   ├── data_convert/                    # Data conversion ✅
+│   │   └── xmlrpcpp/                        # XML-RPC utilities ✅
+│   │
 │   └── APIs/
-│       └── c_api/                   # C API wrapper
-├── build/                            # Build artifacts
-└── doc/                              # Documentation
+│       └── c_api/                           # C API wrapper ✅
+│           ├── include/nav_c_api.h
+│           └── src/nav_c_api.cpp
+│
+├── build/                                   # Build artifacts
+├── config/                                  # Configuration files
+├── examples/                                # Example code
+└── doc/                                     # Documentation
+    ├── architecture_discussion.md
+    ├── implementation_plan.md
+    └── folders.md
 ```

 ## Ghi chú

 - ✅ Kiến trúc cốt lõi đã được triển khai với plugin system linh hoạt
+- ✅ **3 Global Planners** đã triển khai: custom_planner, dock_planner, two_points_planner
+- ✅ **1 Local Planner** đã triển khai: pnkx_local_planner với 4 behaviors (position, docking, go_straight, rotate)
+- ✅ **8 Costmap Layers** đã triển khai: static, obstacle, inflation, voxel, critical, directional, preferred, unpreferred
+- ✅ **MKT Algorithms** hỗ trợ cả differential drive và bicycle model
+- ✅ **Plugin System** hoàn chỉnh với `boost::dll` và `robot::PluginLoaderHelper`
 - ⚠️ Cần bổ sung data sources interfaces và base controller
 - 🔄 Kiến trúc được thiết kế để dễ dàng thay đổi thuật toán và mô hình kinematics thông qua plugin system
- 📦 Tất cả components được build bằng CMake, không phụ thuộc ROS
+- 📦 Tất cả components được build bằng CMake, hỗ trợ cả Catkin và Standalone CMake
 - 🔌 Plugin system sử dụng `boost::dll` cho dynamic loading
+- 🎯 MoveBase đã có control loop (`executeCycle`) và state machine hoàn chỉnh
+- 📚 Có đầy đủ message types: geometry_msgs (29), nav_msgs (24), sensor_msgs (30), std_msgs (32)



--- a/doc/folders.md
+++ b/doc/folders.md
@ -1,53 +1,99 @@
-Mô tả cấu trúc:
+# Cấu trúc thư mục pnkx_nav_core

-├── common_msgs/
-│   ├── build/
-│   ├── geometry_msgs/
-│   │   ├── include/
-│   │   └── test/
-│   ├── CMakeLists.txt
-│   ├── robot_nav_msgs/
-│   │   ├── include/
-│   │   └── test/
-│   ├── CMakeLists.txt
-│   ├── robot_sensor_msgs/
-│   │   ├── cfg/
-│   │   ├── include/
-│   │   └── test/
-│   ├── CMakeLists.txt
-│   ├── robot_std_msgs/
-│   │   ├── include/
-│   │   └── CMakeLists.txt
-│   └── CMakeLists.txt (root)
-|
-├── Navigations/
-│   ├── Cores/
-│   │   └── move_base_core/
-│   │       ├── build/
-│   │       ├── example/
-│   │       ├── include/
-│   │       ├── .gitignore
-│   │       ├── CMakeLists.txt
-│   │       └── README.md
+Mô tả cấu trúc thư mục thực tế của dự án:
+
+```
+pnkx_nav_core/
+├── src/
+│   ├── Navigations/
+│   │   ├── Cores/
+│   │   │   ├── move_base_core/              # BaseNavigation interface
+│   │   │   ├── robot_nav_core/              # Planner interfaces (BaseGlobalPlanner, BaseLocalPlanner, RecoveryBehavior)
+│   │   │   ├── robot_nav_core_adapter/      # Adapter utilities (global_planner_adapter, local_planner_adapter, costmap_adapter)
+│   │   │   └── robot_nav_core2/             # Additional nav utilities (global_planner, local_planner, costmap)
+│   │   ├── Libraries/
+│   │   │   └── nav_grid/                    # Navigation grid utilities
+│   │   └── Packages/
+│   │       └── move_base/                   # MoveBase implementation
+│   │
+│   ├── Algorithms/
+│   │   ├── Cores/
+│   │   │   └── score_algorithm/             # Trajectory scoring và goal checking
+│   │   ├── Libraries/
+│   │   │   ├── mkt_algorithm/               # MKT kinematics (diff drive & bicycle)
+│   │   │   ├── mkt_plugins/                 # MKT plugin components
+│   │   │   ├── mkt_msgs/                    # MKT message types
+│   │   │   ├── kalman/                      # Kalman filtering
+│   │   │   └── angles/                      # Angle utilities
+│   │   └── Packages/
+│   │       ├── global_planners/
+│   │       │   ├── custom_planner/           # Custom global planner
+│   │       │   ├── dock_planner/            # Docking planner
+│   │       │   └── two_points_planner/      # Two points planner
+│   │       └── local_planners/
+│   │           └── pnkx_local_planner/      # PNKX local planner
+│   │
 │   ├── Libraries/
-│   │   ├── geometry_msgs/
-│   │   │   ├── build/
-│   │   │   ├── include/
-│   │   │   └── CMakeLists.txt
-│   │   └── libtf2/
-│   │       ├── .vscode/
-│   │       ├── include/
-│   │       ├── samples/
-│   │       ├── src/
-│   │       ├── .gitignore
-│   │       ├── DEBIAN_PACKAGING.md
-│   │       ├── libtf2_2.0.0_amd64.deb
-│   │       ├── Makefile
-│   │       └── README.md
-│   ├── Packages/
-│   │   └── move_base/
-│   │       ├── build/
-│   │       ├── include/
-│   │       ├── src/
-│   │       └── CMakeLists.txt
-│   └── CMakeLists.txt (root)
+│   │   ├── costmap_2d/                      # Costmap system với 8 layers
+│   │   ├── tf3/                             # Transform system
+│   │   ├── robot_time/                      # Time management
+│   │   ├── robot_cpp/                       # Core utilities (init, NodeHandle, PluginLoaderHelper)
+│   │   ├── robot_nav_2d_utils/             # 2D navigation utilities
+│   │   ├── robot_nav_2d_msgs/              # 2D navigation messages
+│   │   ├── geometry2/                       # Geometry utilities (tf3_geometry_msgs, tf3_sensor_msgs)
+│   │   ├── common_msgs/                     # Message types
+│   │   │   ├── robot_geometry_msgs/         # Geometry messages (29 types)
+│   │   │   ├── robot_nav_msgs/             # Navigation messages (24 types)
+│   │   │   ├── robot_sensor_msgs/          # Sensor messages (30 types)
+│   │   │   ├── robot_std_msgs/             # Standard messages (32 types)
+│   │   │   ├── robot_protocol_msgs/        # Protocol messages
+│   │   │   ├── robot_visualization_msgs/   # Visualization messages
+│   │   │   └── utils/                       # Message utilities
+│   │   ├── laser_geometry/                  # Laser geometry utilities
+│   │   ├── voxel_grid/                      # Voxel grid
+│   │   ├── data_convert/                    # Data conversion
+│   │   └── xmlrpcpp/                        # XML-RPC utilities
+│   │
+│   └── APIs/
+│       └── c_api/                           # C API wrapper cho .NET/C#
+│
+├── build/                                   # Build artifacts
+├── config/                                  # Configuration files
+├── examples/                                # Example code
+└── doc/                                     # Documentation
+    ├── architecture_discussion.md
+    ├── implementation_plan.md
+    ├── folders.md
+    └── readme.md
+```
+
+## Chi tiết các thư mục chính
+
+### Navigations/
+- **Cores/**: Interfaces và core utilities
+  - `move_base_core`: BaseNavigation interface
+  - `robot_nav_core`: Planner interfaces
+  - `robot_nav_core_adapter`: Adapter layer
+  - `robot_nav_core2`: Additional utilities
+- **Libraries/**: Navigation libraries
+  - `nav_grid`: Grid utilities
+- **Packages/**: Implementations
+  - `move_base`: MoveBase implementation
+
+### Algorithms/
+- **Cores/**: Core algorithms
+  - `score_algorithm`: Trajectory scoring
+- **Libraries/**: Algorithm libraries
+  - `mkt_algorithm`: Kinematics algorithms
+  - `mkt_plugins`: Plugin components
+  - `kalman`: Filtering
+- **Packages/**: Algorithm implementations
+  - `global_planners/`: 3 planners
+  - `local_planners/`: 1 planner với multiple behaviors
+
+### Libraries/
+- **costmap_2d/**: Costmap system với 8 layers
+- **tf3/**: Transform system
+- **robot_cpp/**: Core utilities (init, NodeHandle, PluginLoaderHelper)
+- **common_msgs/**: Message type definitions
+- Các utilities khác: time, geometry, sensors, etc.
--- a/src/Libraries/costmap_2d
+++ b/src/Libraries/costmap_2d
@ -1 +1 @@
-Subproject commit 384897b7504103468a5c0501157485c12d8289d8
+Subproject commit 81e78742744c0a5b8a8ef298fa254035c41dc1da
--- a/src/Libraries/robot_cpp/CMakeLists.txt
+++ b/src/Libraries/robot_cpp/CMakeLists.txt
@ -87,6 +87,7 @@ add_library(${PROJECT_NAME} SHARED
  src/console.cpp
  src/node_handle.cpp
  src/plugin_loader_helper.cpp
+  src/init.cpp
 )

 # Enable C++17 filesystem feature
--- a/src/Libraries/robot_cpp/INIT_USAGE.md
+++ b/src/Libraries/robot_cpp/INIT_USAGE.md
@ -0,0 +1,483 @@
+# robot::ok() - Hướng dẫn sử dụng
+
+## Tổng quan
+
+`robot::ok()` và các hàm liên quan cung cấp cơ chế quản lý vòng đời của robot system, tương tự như `ros::ok()` trong ROS. Chúng cho phép bạn kiểm tra trạng thái system và xử lý shutdown một cách graceful.
+
+## API Reference
+
+### `bool robot::ok()`
+
+Kiểm tra xem robot system có đang chạy không.
+
+- **Trả về `true`**: System đang chạy bình thường
+- **Trả về `false`**: System đã shutdown hoàn toàn
+
+### `bool robot::isShuttingDown()`
+
+Kiểm tra xem shutdown đã được yêu cầu chưa.
+
+- **Trả về `true`**: Shutdown đã được yêu cầu (ngay lập tức)
+- **Trả về `false`**: System vẫn đang chạy
+
+### `void robot::shutdown()`
+
+Bắt đầu quá trình shutdown system.
+
+### `void robot::init(int& argc, char** argv, const std::string& node_name = "", bool install_sigint_handler = true)`
+
+Khởi tạo robot system.
+
+- `argc`, `argv`: Command line arguments (có thể được modify)
+- `node_name`: Tên node (optional, dùng cho logging)
+- `install_sigint_handler`: Nếu `true`, tự động xử lý Ctrl-C (default: `true`)
+
+### `void robot::init(const std::string& node_name = "", bool install_sigint_handler = true)`
+
+Khởi tạo robot system không cần command line arguments.
+
+## Cách sử dụng cơ bản
+
+### 1. Main Loop với Rate
+
+```cpp
+#include <robot/init.h>
+#include <robot/time.h>
+
+int main(int argc, char** argv)
+{
+  // Initialize robot system
+  robot::init(argc, argv, "my_node");
+  
+  robot::Rate rate(10);  // 10 Hz
+  
+  // Main loop
+  while (robot::ok())
+  {
+    // Do work here
+    std::cout << "Running..." << std::endl;
+    
+    rate.sleep();
+  }
+  
+  return 0;
+}
+```
+
+### 2. Main Loop không dùng Rate
+
+```cpp
+#include <robot/init.h>
+#include <robot/time.h>
+
+int main(int argc, char** argv)
+{
+  robot::init(argc, argv, "my_node");
+  
+  while (robot::ok())
+  {
+    // Do work
+    robot::Duration(0.1).sleep();  // Sleep 100ms
+  }
+  
+  return 0;
+}
+```
+
+### 3. Thread Loop
+
+```cpp
+#include <robot/init.h>
+#include <thread>
+#include <atomic>
+
+std::atomic<bool> stop_flag_(false);
+
+void myThread()
+{
+  while (robot::ok() && !stop_flag_)
+  {
+    // Do work in thread
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+  }
+}
+
+int main(int argc, char** argv)
+{
+  robot::init(argc, argv, "my_node");
+  
+  std::thread t(myThread);
+  
+  // Main loop
+  while (robot::ok())
+  {
+    // Do main work
+    robot::Duration(1.0).sleep();
+  }
+  
+  stop_flag_ = true;
+  t.join();
+  
+  return 0;
+}
+```
+
+### 4. Long-running Callback với Early Exit
+
+```cpp
+#include <robot/init.h>
+
+void processLargeData(const std::vector<int>& data)
+{
+  for (size_t i = 0; i < data.size(); i++)
+  {
+    // Check if shutdown requested
+    if (robot::isShuttingDown())
+    {
+      std::cout << "Shutdown requested, exiting early" << std::endl;
+      return;  // Exit early
+    }
+    
+    // Process data[i]...
+    processItem(data[i]);
+  }
+}
+
+int main(int argc, char** argv)
+{
+  robot::init(argc, argv, "my_node");
+  
+  std::vector<int> large_data(1000000);
+  // ... fill data ...
+  
+  while (robot::ok())
+  {
+    processLargeData(large_data);
+    robot::Duration(1.0).sleep();
+  }
+  
+  return 0;
+}
+```
+
+### 5. Custom SIGINT Handler
+
+```cpp
+#include <robot/init.h>
+#include <signal.h>
+#include <robot/time.h>
+
+void mySigintHandler(int sig)
+{
+  (void)sig;
+  
+  // Do custom cleanup
+  std::cout << "Custom cleanup before shutdown..." << std::endl;
+  
+  // Publish stop message, save state, etc.
+  
+  // Call shutdown
+  robot::shutdown();
+}
+
+int main(int argc, char** argv)
+{
+  // Initialize without SIGINT handler
+  robot::init(argc, argv, "my_node", false);
+  
+  // Install custom handler
+  signal(SIGINT, mySigintHandler);
+  
+  while (robot::ok())
+  {
+    // Do work
+    robot::Duration(0.1).sleep();
+  }
+  
+  return 0;
+}
+```
+
+### 6. Nested Loops
+
+```cpp
+#include <robot/init.h>
+#include <robot/time.h>
+
+int main(int argc, char** argv)
+{
+  robot::init(argc, argv, "my_node");
+  
+  while (robot::ok())  // Outer loop
+  {
+    // Setup connection
+    bool connected = connectToDevice();
+    
+    while (robot::ok() && connected)  // Inner loop
+    {
+      // Do work with device
+      if (!deviceIsAlive())
+      {
+        connected = false;
+        break;
+      }
+      
+      robot::Duration(0.1).sleep();
+    }
+    
+    // Reconnect after delay
+    robot::Duration(1.0).sleep();
+  }
+  
+  return 0;
+}
+```
+
+### 7. Multiple Conditions
+
+```cpp
+#include <robot/init.h>
+#include <robot/time.h>
+#include <atomic>
+
+std::atomic<bool> cancel_(false);
+std::atomic<bool> connected_(false);
+
+int main(int argc, char** argv)
+{
+  robot::init(argc, argv, "my_node");
+  
+  while (robot::ok() && !cancel_ && connected_)
+  {
+    // Do work only if all conditions are met
+    processData();
+    
+    robot::Duration(0.1).sleep();
+  }
+  
+  return 0;
+}
+```
+
+## So sánh với ROS
+
+### Tương đương với ROS:
+
+| ROS | robot:: | Mô tả |
+|-----|---------|-------|
+| `ros::ok()` | `robot::ok()` | Kiểm tra system đang chạy |
+| `ros::isShuttingDown()` | `robot::isShuttingDown()` | Kiểm tra shutdown được yêu cầu |
+| `ros::shutdown()` | `robot::shutdown()` | Bắt đầu shutdown |
+| `ros::init(argc, argv, "node")` | `robot::init(argc, argv, "node")` | Khởi tạo system |
+
+### Ví dụ chuyển đổi từ ROS:
+
+**ROS Code:**
+```cpp
+#include <ros/ros.h>
+
+int main(int argc, char** argv)
+{
+  ros::init(argc, argv, "my_node");
+  ros::NodeHandle nh;
+  ros::Rate rate(10);
+  
+  while (ros::ok())
+  {
+    ros::spinOnce();
+    rate.sleep();
+  }
+  
+  return 0;
+}
+```
+
+**robot:: Code:**
+```cpp
+#include <robot/init.h>
+#include <robot/time.h>
+
+int main(int argc, char** argv)
+{
+  robot::init(argc, argv, "my_node");
+  robot::Rate rate(10);
+  
+  while (robot::ok())
+  {
+    // Do work (no ros::spinOnce() equivalent needed)
+    rate.sleep();
+  }
+  
+  return 0;
+}
+```
+
+## Best Practices
+
+### ✅ Nên làm:
+
+1. **Luôn gọi `robot::init()` trước khi dùng các hàm khác**
+   ```cpp
+   int main(int argc, char** argv)
+   {
+     robot::init(argc, argv, "my_node");
+     // ... rest of code
+   }
+   ```
+
+2. **Luôn kiểm tra `robot::ok()` trong vòng lặp chính**
+   ```cpp
+   while (robot::ok())
+   {
+     // Do work
+   }
+   ```
+
+3. **Kiểm tra trong thread loops**
+   ```cpp
+   while (robot::ok() && !stop_flag_)
+   {
+     // Thread work
+   }
+   ```
+
+4. **Dùng `robot::isShuttingDown()` trong callback dài**
+   ```cpp
+   if (robot::isShuttingDown()) return;
+   ```
+
+5. **Kết hợp với các điều kiện khác**
+   ```cpp
+   while (robot::ok() && connected && !cancel_)
+   {
+     // Work
+   }
+   ```
+
+### ❌ Không nên:
+
+1. **Không bỏ qua kiểm tra trong thread**
+   - Thread có thể chạy mãi nếu không kiểm tra
+
+2. **Không dùng `robot::ok()` trong callback ngắn**
+   - Callback ngắn sẽ chạy xong trước khi shutdown
+   - Dùng `robot::isShuttingDown()` thay vì
+
+3. **Không gọi `robot::shutdown()` trong callback**
+   - Có thể gây deadlock
+   - Để shutdown tự nhiên hoặc dùng flag
+
+## Shutdown Process
+
+### Quy trình shutdown:
+
+1. **SIGINT (Ctrl-C) được gửi** hoặc `robot::shutdown()` được gọi
+2. **`robot::isShuttingDown()`** → `true` (ngay lập tức)
+3. **`robot::ok()`** → vẫn `true` (chưa shutdown hoàn toàn)
+4. **Resources được cleanup** (nếu có)
+5. **`robot::ok()`** → `false` (shutdown hoàn tất)
+
+### Timeline:
+
+```
+Time    | Event                    | ok() | isShuttingDown()
+--------|--------------------------|------|------------------
+T0      | System running           | true | false
+T1      | Ctrl-C pressed           | true | false
+T2      | shutdown() called        | true | true  ← isShuttingDown() = true
+T3      | Cleanup in progress      | true | true
+T4      | Cleanup complete         | false| true  ← ok() = false
+```
+
+## Ví dụ thực tế trong move_base
+
+Bạn có thể sử dụng trong `planThread()`:
+
+```cpp
+#include <robot/init.h>
+
+void move_base::MoveBase::planThread()
+{
+  robot::Timer timer;
+  bool wait_for_wake = false;
+  boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
+  
+  while (robot::ok())  // ← Thêm kiểm tra này
+  {
+    // ... existing code ...
+    
+    while (wait_for_wake || !runPlanner_)
+    {
+      if (!robot::ok())  // ← Kiểm tra trước khi wait
+        break;
+        
+      planner_cond_.wait(lock);
+      wait_for_wake = false;
+    }
+    
+    // ... rest of code ...
+  }
+}
+```
+
+## Troubleshooting
+
+### Vấn đề: Thread không dừng khi shutdown
+
+**Nguyên nhân**: Không kiểm tra `robot::ok()` trong thread loop
+
+**Giải pháp**:
+```cpp
+void myThread()
+{
+  while (robot::ok() && !stop_flag_)  // ← Thêm robot::ok()
+  {
+    // Work
+  }
+}
+```
+
+### Vấn đề: Callback dài không exit khi shutdown
+
+**Nguyên nhân**: Không kiểm tra `robot::isShuttingDown()`
+
+**Giải pháp**:
+```cpp
+void longCallback()
+{
+  for (int i = 0; i < 1000000; i++)
+  {
+    if (robot::isShuttingDown())  // ← Thêm kiểm tra
+      return;
+    // Work
+  }
+}
+```
+
+### Vấn đề: Shutdown không hoạt động
+
+**Nguyên nhân**: Chưa gọi `robot::init()`
+
+**Giải pháp**:
+```cpp
+int main(int argc, char** argv)
+{
+  robot::init(argc, argv, "my_node");  // ← Phải gọi init() trước
+  // ...
+}
+```
+
+## Tóm tắt
+
+- **`robot::ok()`**: Kiểm tra system đã shutdown hoàn toàn chưa → dùng trong loops
+- **`robot::isShuttingDown()`**: Kiểm tra shutdown đã được yêu cầu chưa → dùng trong callback dài
+- **`robot::shutdown()`**: Bắt đầu shutdown process
+- **`robot::init()`**: Khởi tạo system (phải gọi trước khi dùng các hàm khác)
+- **Luôn kiểm tra** trong vòng lặp chính và thread loops
+- **Mặc định** tự động xử lý SIGINT (Ctrl-C)
+
+## Tài liệu tham khảo
+
+- [ROS Wiki - Initialization and Shutdown](https://wiki.ros.org/roscpp/Overview/Initialization%20and%20Shutdown)
+- `robot_time/ROS_OK_EXPLANATION.md` - Giải thích chi tiết về `ros::ok()`
+
--- a/src/Libraries/robot_cpp/README.md
+++ b/src/Libraries/robot_cpp/README.md
@ -1,14 +1,124 @@
 # robot_cpp

-Thư viện C++ cung cấp các công cụ tiện ích cho phát triển robot, bao gồm hệ thống logging có màu sắc và quản lý tham số dựa trên YAML (tương tự ROS parameter server).
+## Tổng quan

-## Mô tả
+`robot_cpp` là một thư viện C++ cung cấp các công cụ tiện ích cốt lõi cho phát triển ứng dụng robot, tương tự như các utilities trong ROS nhưng hoàn toàn độc lập, không phụ thuộc vào ROS. Thư viện này cung cấp nền tảng cơ bản cho việc xây dựng các hệ thống robot tự động.

-`robot_cpp` là một thư viện hỗ trợ phát triển ứng dụng robot với hai module chính:
+## Mục đích và phạm vi ứng dụng

-1. **Console Module** (`robot::console`): Hệ thống logging với hỗ trợ màu sắc ANSI, tự động phát hiện terminal, và throttle logging để tránh spam console.
+Thư viện `robot_cpp` được thiết kế để giải quyết các nhu cầu cơ bản trong phát triển ứng dụng robot:

-2. **NodeHandle Module** (`robot::NodeHandle`): Interface quản lý tham số giống ROS, sử dụng YAML files làm backend, hỗ trợ namespace và cấu trúc tham số phân cấp.
+- **Logging và Debugging**: Cung cấp hệ thống logging với màu sắc để dễ dàng phân biệt các loại thông điệp
+- **Quản lý cấu hình**: Hỗ trợ quản lý tham số từ file YAML, tương tự ROS parameter server
+- **Quản lý vòng đời**: Cung cấp cơ chế quản lý lifecycle của hệ thống robot, xử lý shutdown graceful
+- **Plugin Loading**: Hỗ trợ tìm kiếm và load các dynamic plugins sử dụng boost::dll
+
+## Các thành phần chính
+
+### 1. Console Module - Hệ thống Logging
+
+Module Console cung cấp hệ thống logging với hỗ trợ màu sắc ANSI để phân biệt các loại thông điệp.
+
+**Đặc điểm:**
+- Hỗ trợ nhiều mức độ logging: info, success, warning, error, debug
+- Tự động phát hiện khả năng hỗ trợ màu của terminal
+- Throttle logging để giới hạn tần suất log, tránh spam console
+- Hỗ trợ logging với thông tin file và line number để debug
+- Tự động reset màu sau khi in để tránh ảnh hưởng đến output tiếp theo
+- Có thể bật/tắt màu sắc tùy theo môi trường
+
+**Cơ chế hoạt động:**
+- Kiểm tra biến môi trường `NO_COLOR` và `TERM` để xác định hỗ trợ màu
+- Sử dụng static variables để lưu timestamp cho throttle logging
+- Tự động thêm newline nếu format string không kết thúc bằng newline
+- Hỗ trợ nhiều màu sắc: red, green, yellow, blue, cyan, magenta, white và các biến thể bright
+
+**Ứng dụng:**
+- Debug và monitoring trong quá trình phát triển
+- Hiển thị trạng thái hệ thống với màu sắc dễ phân biệt
+- Logging có điều kiện với throttle để tránh spam
+- Tích hợp vào các hệ thống logging lớn hơn
+
+### 2. NodeHandle Module - Quản lý Tham số
+
+Module NodeHandle cung cấp interface quản lý tham số giống ROS parameter server, sử dụng YAML files làm backend.
+
+**Đặc điểm:**
+- Hỗ trợ namespace phân cấp giống ROS
+- Tự động load YAML files từ config directory
+- Hỗ trợ nhiều kiểu dữ liệu: bool, int, double, float, string, vector, map
+- Merge nhiều YAML files vào một parameter tree
+- Thread-safe parameter access
+- Hỗ trợ nested parameters với cấu trúc phân cấp
+
+**Cơ chế hoạt động:**
+- Sử dụng `YAML::Node` để lưu trữ tham số trong memory
+- Static `root_` node chứa toàn bộ parameter tree, được chia sẻ giữa tất cả NodeHandle instances
+- Mỗi NodeHandle instance scope vào một namespace cụ thể
+- Tự động tìm và load YAML files từ config directory khi khởi tạo
+- Hỗ trợ remapping parameters tương tự ROS
+
+**Namespace System:**
+- Root namespace (`""` hoặc `"/"`): Truy cập toàn bộ parameter tree
+- Private namespace (`"~"`): Map tới config directory của node hiện tại
+- Nested namespaces: Sử dụng `/` separator (ví dụ: `"robot/base/velocity"`)
+- Tất cả NodeHandle instances chia sẻ cùng một static parameter tree
+
+**Ứng dụng:**
+- Quản lý cấu hình robot từ file YAML
+- Truyền tham số giữa các modules
+- Runtime configuration changes
+- Testing với các bộ tham số khác nhau
+
+### 3. Init Module - Quản lý Vòng đời Hệ thống
+
+Module Init quản lý vòng đời của robot system, tương tự `ros::init()` và `ros::ok()` trong ROS.
+
+**Đặc điểm:**
+- Quản lý trạng thái system (đang chạy hay đã shutdown)
+- Xử lý shutdown graceful với signal handling
+- Tự động xử lý SIGINT (Ctrl-C) để shutdown an toàn
+- Hỗ trợ custom signal handler
+- Thread-safe với `std::atomic`
+
+**Cơ chế hoạt động:**
+- Sử dụng `std::atomic` để đảm bảo thread-safety
+- Tự động cài đặt SIGINT handler để xử lý Ctrl-C
+- Quản lý trạng thái shutdown với 2 flags: `shutdown_requested` và `shutdown_complete`
+- Hỗ trợ custom signal handler nếu cần xử lý đặc biệt
+
+**Các trạng thái:**
+- **Running**: System đang chạy bình thường
+- **Shutdown Requested**: Shutdown đã được yêu cầu (ngay lập tức khi gọi `shutdown()`)
+- **Shutdown Complete**: System đã shutdown hoàn toàn
+
+**Ứng dụng:**
+- Main loops với kiểm tra `robot::ok()`
+- Thread loops cần kiểm tra trạng thái system
+- Long-running callbacks cần early exit khi shutdown
+- Graceful shutdown với cleanup resources
+
+### 4. PluginLoaderHelper Module - Tìm kiếm Plugin Libraries
+
+Module PluginLoaderHelper giúp tìm đường dẫn library file (.so) từ tên symbol (export name) được sử dụng với `BOOST_DLL_ALIAS`.
+
+**Đặc điểm:**
+- Map tên symbol (export name) sang đường dẫn library file
+- Đọc cấu hình từ YAML file hoặc NodeHandle
+- Tự động tìm build directory tại runtime
+- Hỗ trợ nhiều cách tìm kiếm: từ config, từ build directory, từ system paths
+
+**Cơ chế hoạt động:**
+- Đọc file YAML (`boost_dll_plugins.yaml`) hoặc từ NodeHandle
+- Map symbol names (ví dụ: "CustomPlanner") sang library paths
+- Tự động tìm build directory từ environment variables hoặc config
+- Hỗ trợ fallback mechanisms nếu không tìm thấy trong config
+
+**Ứng dụng:**
+- Dynamic loading của global planners, local planners, recovery behaviors
+- Plugin discovery trong runtime
+- Tích hợp với boost::dll để load plugins
+- Quản lý plugin registry

 ## Kiến trúc

@ -17,358 +127,126 @@ Thư viện C++ cung cấp các công cụ tiện ích cho phát triển robot,
 ```
 robot_cpp/
 ├── include/robot/
-│   ├── console.h          # Console logging API
-│   └── node_handle.h     # NodeHandle parameter management API
+│   ├── console.h              # Console logging API
+│   ├── node_handle.h          # NodeHandle parameter management API
+│   ├── init.h                 # System initialization and shutdown API
+│   └── plugin_loader_helper.h # Plugin loader helper API
 ├── src/
-│   ├── console.cpp        # Console implementation
-│   └── node_handle.cpp   # NodeHandle implementation
+│   ├── console.cpp            # Console implementation
+│   ├── node_handle.cpp        # NodeHandle implementation
+│   ├── init.cpp               # Init implementation
+│   └── plugin_loader_helper.cpp # PluginLoaderHelper implementation
 ├── test/
-│   └── test_node_handle.cpp  # Unit tests
+│   └── test_node_handle.cpp   # Unit tests
+├── INIT_USAGE.md              # Hướng dẫn chi tiết về Init module
+├── PLUGIN_LOADER_README.md    # Hướng dẫn về PluginLoaderHelper
 └── CMakeLists.txt
 ```

-### Module Console
+### Mối quan hệ giữa các modules

-**Namespace:** `robot::console` và `robot::color`
-
-**Chức năng:**
- Logging với màu sắc ANSI (red, green, yellow, blue, cyan, magenta, white)
- Tự động phát hiện hỗ trợ màu của terminal
- Throttle logging để giới hạn tần suất log
- Hỗ trợ logging với thông tin file và line number
- Tự động reset màu sau khi in
-
-**Cơ chế hoạt động:**
- Kiểm tra biến môi trường `NO_COLOR` và `TERM` để xác định hỗ trợ màu
- Sử dụng static variables để lưu timestamp cho throttle logging
- Tự động thêm newline nếu format string không kết thúc bằng `\n`
-
-### Module NodeHandle
-
-**Class:** `robot::NodeHandle`
-
-**Chức năng:**
- Quản lý tham số từ YAML files
- Hỗ trợ namespace phân cấp (giống ROS)
- Tự động load YAML files từ config directory
- Hỗ trợ nhiều kiểu dữ liệu: bool, int, double, float, string, vector, map
- Merge nhiều YAML files vào một parameter tree
-
-**Cơ chế hoạt động:**
- Sử dụng `YAML::Node` để lưu trữ tham số
- Static `root_` node chứa toàn bộ parameter tree
- Mỗi NodeHandle instance scope vào một namespace cụ thể
- Tự động tìm và load YAML files từ config directory
-
-## Cách sử dụng
-
-### 1. Console Logging
-
-#### Basic Logging
-
-```cpp
-#include "robot/console.h"
-
-using namespace robot;
-
-// Log thông tin (màu trắng)
-log_info("Processing %d items\n", count);
-
-// Log thành công (màu xanh lá)
-log_success("Operation completed successfully\n");
-
-// Log cảnh báo (màu vàng)
-log_warning("Low battery: %d%%\n", battery_level);
-
-// Log lỗi (màu đỏ)
-log_error("Failed to open file: %s\n", filename);
-
-// Log debug (màu cyan)
-log_debug("Variable value: %d\n", value);
-```
-
-#### Logging với File và Line Number
-
-```cpp
-// Tự động thêm thông tin file và line number
-log_info_at(__FILE__, __LINE__, "Processing started\n");
-log_error_at(__FILE__, __LINE__, "Error occurred: %s\n", error_msg);
-```
-
-#### Throttle Logging
-
-```cpp
-// Log tối đa 1 lần mỗi giây
-log_error_throttle(1.0, "Connection failed: %s\n", error_msg);
-
-// Log tối đa 10 lần mỗi giây (0.1 giây)
-log_warning_throttle(0.1, "Low battery: %d%%\n", battery_level);
-
-// Log info với throttle
-log_info_throttle(2.0, "Status update: %s\n", status);
-```
-
-#### Custom Color Printing
-
-```cpp
-// In với màu tùy chỉnh
-printf_red("Critical error!\n");
-printf_green("Success!\n");
-printf_yellow("Warning!\n");
-
-// Sử dụng color namespace
-printf_color(color::BRIGHT_RED, "Critical: %s\n", message);
-printf_color(color::BOLD, "Important message\n");
-printf_color(color::UNDERLINE, "Underlined text\n");
-```
-
-#### Điều khiển màu sắc
-
-```cpp
-// Kiểm tra hỗ trợ màu
-if (is_color_supported()) {
-    log_info("Terminal supports colors\n");
-}
-
-// Tắt màu (ví dụ khi redirect output)
-set_color_enabled(false);
-log_info("This will be plain text\n");
-```
-
-### 2. NodeHandle Parameter Management
-
-#### Khởi tạo NodeHandle
-
-```cpp
-#include "robot/node_handle.h"
-
-using namespace robot;
-
-// Root namespace
-NodeHandle nh;
-
-// Namespace cụ thể
-NodeHandle nh_robot("/robot");
-
-// Private namespace (tự động map tới config directory)
-NodeHandle nh_private("~");
-
-// Từ parent NodeHandle
-NodeHandle nh_base(nh_robot, "/base");
-```
-
-#### Đọc tham số
-
-```cpp
-// Sử dụng param() với default value (khuyến nghị)
-int max_iterations = nh.param("max_iterations", 100);
-double tolerance = nh.param("tolerance", 0.1);
-std::string frame_id = nh.param("frame_id", std::string("base_link"));
-bool use_odom = nh.param("use_odometry", true);
-
-// Sử dụng getParam() với output parameter
-int value;
-if (nh.getParam("my_param", value, 0)) {
-    // Parameter tồn tại
-} else {
-    // Sử dụng default value (0)
-}
-
-// Đọc nested parameters
-double velocity = nh.param("robot/base/max_velocity", 1.0);
-
-// Đọc vector
-std::vector<double> limits = nh.param("limits", std::vector<double>{0.0, 1.0, 2.0});
-
-// Đọc map
-std::map<std::string, double> weights;
-nh.getParam("weights", weights);
-```
-
-#### Ghi tham số
-
-```cpp
-// Set các kiểu dữ liệu khác nhau
-nh.setParam("max_iterations", 200);
-nh.setParam("frame_id", std::string("odom"));
-nh.setParam("enabled", true);
-
-// Set nested parameter
-nh.setParam("robot/base/max_velocity", 2.0);
-
-// Set vector
-std::vector<int> values = {1, 2, 3, 4};
-nh.setParam("values", values);
-
-// Set map
-std::map<std::string, double> config;
-config["weight1"] = 0.5;
-config["weight2"] = 0.3;
-nh.setParam("config", config);
-```
-
-#### Kiểm tra tham số
-
-```cpp
-// Kiểm tra tồn tại
-if (nh.hasParam("my_param")) {
-    // Parameter tồn tại
-}
-
-// Tìm kiếm parameter
-std::string result;
-if (nh.searchParam("my_param", result)) {
-    // Tìm thấy tại: result
-}
-```
-
-#### Load YAML Files
-
-```cpp
-// Tự động load từ config directory (được gọi trong constructor)
-NodeHandle nh;  // Tự động load YAML files
-
-// Load thủ công từ directory
-int loaded = nh.loadYamlFilesFromDirectory("/path/to/config");
-
-// In tất cả parameters (debug)
-nh.printNodeParams();
-```
-
-#### Ví dụ YAML Configuration
-
-```yaml
-# config/robot_params.yaml
-robot:
-  base:
-    max_velocity: 2.0
-    max_acceleration: 1.0
-    frame_id: "base_link"
-  
-  sensors:
-    laser:
-      enabled: true
-      range: 10.0
-      frequency: 10.0
-    
-    camera:
-      enabled: false
-      resolution: [1920, 1080]
-
-  limits:
-    - 0.0
-    - 1.0
-    - 2.0
-```
-
-Sử dụng trong code:
-
-```cpp
-NodeHandle nh;
-
-// Đọc từ YAML
-double max_vel = nh.param("robot/base/max_velocity", 1.0);
-bool laser_enabled = nh.param("robot/sensors/laser/enabled", false);
-std::vector<double> limits = nh.param("robot/limits", std::vector<double>());
-```
-
-### 3. Ví dụ tích hợp
-
-```cpp
-#include "robot/console.h"
-#include "robot/node_handle.h"
-
-int main() {
-    using namespace robot;
-    
-    // Khởi tạo NodeHandle và load config
-    NodeHandle nh;
-    
-    // Đọc tham số
-    int max_iter = nh.param("max_iterations", 100);
-    double tolerance = nh.param("tolerance", 0.1);
-    
-    log_info("Starting with max_iterations=%d, tolerance=%.2f\n", 
-             max_iter, tolerance);
-    
-    // Vòng lặp xử lý
-    for (int i = 0; i < max_iter; ++i) {
-        // Throttle log để tránh spam
-        log_info_throttle(1.0, "Iteration %d/%d\n", i+1, max_iter);
-        
-        // Xử lý...
-        
-        if (some_error_condition) {
-            log_error_throttle(0.5, "Error at iteration %d\n", i);
-        }
-    }
-    
-    log_success("Processing completed\n");
-    
-    return 0;
-}
-```
+- **Console** là module độc lập, không phụ thuộc vào modules khác
+- **NodeHandle** sử dụng yaml-cpp để parse YAML files
+- **Init** quản lý lifecycle chung của system, được sử dụng bởi tất cả modules khác
+- **PluginLoaderHelper** sử dụng NodeHandle để đọc cấu hình plugins

 ## Dependencies

- **yaml-cpp**: Thư viện parse YAML files
- **robot_xmlrpcpp**: Hỗ trợ XmlRpcValue conversion (optional)
+- **yaml-cpp**: Thư viện parse YAML files (cho NodeHandle và PluginLoaderHelper)
+- **robot_xmlrpcpp**: Hỗ trợ XmlRpcValue conversion (optional, cho NodeHandle)
+- **robot_time**: Thư viện quản lý thời gian (cho Init module với Rate/Duration)
+- **C++17**: Yêu cầu C++ standard 17 trở lên
+- **Boost.DLL**: Được sử dụng bởi PluginLoaderHelper (thông qua boost::dll)

-## Build
+## Build và Installation

-Thư viện được build cùng với project chính thông qua CMake:
+Thư viện hỗ trợ cả Catkin và Standalone CMake:

-```bash
-cd build
-cmake ..
-make
-```
+- **Với Catkin**: Tích hợp vào catkin workspace và build như các ROS packages khác
+- **Với Standalone CMake**: Có thể build độc lập, không cần ROS

-## API Reference
+## Use Cases

-### Console Functions
+### Configuration Management
+Sử dụng NodeHandle để quản lý tất cả cấu hình robot từ file YAML, cho phép thay đổi cấu hình mà không cần recompile.

-| Function | Mô tả |
-|----------|-------|
-| `log_info(format, ...)` | Log thông tin (màu trắng) |
-| `log_success(format, ...)` | Log thành công (màu xanh lá) |
-| `log_warning(format, ...)` | Log cảnh báo (màu vàng) |
-| `log_error(format, ...)` | Log lỗi (màu đỏ) |
-| `log_debug(format, ...)` | Log debug (màu cyan) |
-| `log_*_at(file, line, format, ...)` | Log với file và line number |
-| `log_*_throttle(period, format, ...)` | Log với throttle (giới hạn tần suất) |
-| `printf_*()` | In với màu cụ thể |
-| `is_color_enabled()` | Kiểm tra màu có được bật |
-| `set_color_enabled(bool)` | Bật/tắt màu |
+### System Logging
+Sử dụng Console module để log thông tin, cảnh báo, và lỗi với màu sắc, giúp dễ dàng debug và monitor hệ thống.

-### NodeHandle Methods
+### Lifecycle Management
+Sử dụng Init module để quản lý vòng đời của ứng dụng, đảm bảo shutdown graceful khi nhận Ctrl-C hoặc signal khác.

-| Method | Mô tả |
-|--------|-------|
-| `param<T>(key, default)` | Đọc tham số với default value |
-| `getParam(key, value, default)` | Đọc tham số với output parameter |
-| `setParam(key, value)` | Ghi tham số |
-| `hasParam(key)` | Kiểm tra tham số tồn tại |
-| `searchParam(key, result)` | Tìm kiếm tham số |
-| `loadYamlFilesFromDirectory(path)` | Load YAML files từ directory |
-| `printNodeParams()` | In tất cả tham số (debug) |
+### Plugin Discovery
+Sử dụng PluginLoaderHelper để tìm và load các plugins động tại runtime, cho phép mở rộng hệ thống mà không cần recompile.

-## Lưu ý
+### Multi-threaded Applications
+Tất cả các modules đều thread-safe, cho phép sử dụng an toàn trong các ứng dụng đa luồng.

-1. **Throttle Logging**: Mỗi hàm throttle sử dụng static variable riêng để lưu timestamp. Các lời gọi từ cùng một vị trí code sẽ chia sẻ cùng một throttle counter.
+## Best Practices

-2. **Color Support**: Màu sắc tự động bị tắt nếu:
-   - Biến môi trường `NO_COLOR` được set
-   - Terminal không hỗ trợ ANSI colors
-   - Output được redirect vào file
+1. **Sử dụng NodeHandle cho tất cả cấu hình**: Tránh hardcode parameters, sử dụng YAML files

-3. **NodeHandle Namespace**: 
-   - Namespace `"~"` hoặc `""` map tới root namespace `/`
-   - Nested parameters sử dụng `/` separator (ví dụ: `"robot/base/velocity"`)
-   - Tất cả NodeHandle instances chia sẻ cùng một static parameter tree
+2. **Sử dụng Console logging thay vì printf/cout**: Có màu sắc và throttle logging tự động

-4. **YAML Loading**: NodeHandle tự động tìm và load YAML files từ config directory khi khởi tạo. Files được merge theo thứ tự filesystem.
+3. **Luôn kiểm tra robot::ok() trong loops**: Đảm bảo có thể shutdown gracefully
+
+4. **Sử dụng throttle logging cho frequent messages**: Tránh spam console với các messages lặp lại
+
+5. **Namespace organization**: Sử dụng namespace phân cấp để tổ chức parameters một cách logic
+
+6. **Plugin configuration**: Sử dụng PluginLoaderHelper thay vì hardcode library paths
+
+## Tính năng nổi bật
+
+### 1. Color Support với Auto-detection
+Console module tự động phát hiện khả năng hỗ trợ màu của terminal và tự động tắt màu nếu:
+- Biến môi trường `NO_COLOR` được set
+- Terminal không hỗ trợ ANSI colors
+- Output được redirect vào file
+
+### 2. Throttle Logging
+Mỗi hàm throttle sử dụng static variable riêng để lưu timestamp, cho phép:
+- Giới hạn tần suất log của các messages lặp lại
+- Tránh spam console với các messages thường xuyên
+- Tự động reset sau một khoảng thời gian
+
+### 3. YAML Auto-loading
+NodeHandle tự động tìm và load YAML files từ config directory khi khởi tạo:
+- Tìm tất cả `.yaml` và `.yml` files trong config directory
+- Merge tất cả files vào một parameter tree
+- Hỗ trợ nested structures và arrays
+
+### 4. Thread-safe Operations
+Tất cả các operations đều thread-safe:
+- NodeHandle sử dụng static parameter tree với thread-safe access
+- Init module sử dụng `std::atomic` cho flags
+- Console logging thread-safe với static variables
+
+### 5. Graceful Shutdown
+Init module cung cấp cơ chế shutdown graceful:
+- Tự động xử lý SIGINT (Ctrl-C)
+- Hỗ trợ custom signal handlers
+- Phân biệt giữa "shutdown requested" và "shutdown complete"
+- Cho phép early exit trong long-running callbacks
+
+## Lưu ý quan trọng
+
+- **NodeHandle static tree**: Tất cả NodeHandle instances chia sẻ cùng một static parameter tree, thay đổi từ một instance sẽ ảnh hưởng đến tất cả instances khác
+
+- **YAML loading order**: YAML files được load và merge theo thứ tự filesystem, file sau sẽ override parameters của file trước nếu có conflict
+
+- **Throttle logging scope**: Mỗi hàm throttle sử dụng static variable riêng, các lời gọi từ cùng một vị trí code sẽ chia sẻ cùng một throttle counter
+
+- **Plugin path resolution**: PluginLoaderHelper tìm library paths theo thứ tự: config file → build directory → system paths
+
+- **Init timing**: Cần gọi `robot::init()` trước khi sử dụng các modules khác, đặc biệt là `robot::Time::now()`
+
+## Tài liệu tham khảo
+
+- `INIT_USAGE.md` - Hướng dẫn chi tiết về Init module với các use cases và best practices
+- `PLUGIN_LOADER_README.md` - Hướng dẫn về PluginLoaderHelper và cách cấu hình plugins
+- `README.md` (file này) - Tổng quan về toàn bộ thư viện

 ## License

-Xem file LICENSE trong thư mục gốc.
+BSD License - Xem file LICENSE trong thư mục gốc.
--- a/src/Libraries/robot_cpp/include/robot/init.h
+++ b/src/Libraries/robot_cpp/include/robot/init.h
@ -0,0 +1,134 @@
+#ifndef ROBOT_INIT_H_INCLUDED
+#define ROBOT_INIT_H_INCLUDED
+
+#include <atomic>
+#include <csignal>
+#include <string>
+
+namespace robot
+{
+
+/**
+ * @brief Check if the robot system is still running
+ * 
+ * Similar to ros::ok(), this function returns false when the system
+ * has finished shutting down. Use this in main loops and thread loops
+ * to check if the system should continue running.
+ * 
+ * @return true if system is running, false if shutdown is complete
+ * 
+ * @note This returns false only after shutdown is complete, not when
+ *       shutdown is requested. Use isShuttingDown() to check if shutdown
+ *       has been requested.
+ * 
+ * Example:
+ * @code
+ * while (robot::ok())
+ * {
+ *   // Do work
+ * }
+ * @endcode
+ */
+bool ok();
+
+/**
+ * @brief Check if shutdown has been requested
+ * 
+ * Similar to ros::isShuttingDown(), this function returns true as soon
+ * as shutdown() is called, not when shutdown is complete. Use this in
+ * long-running callbacks to check if they should exit early.
+ * 
+ * @return true if shutdown has been requested, false otherwise
+ * 
+ * @note This returns true immediately when shutdown() is called, while
+ *       ok() returns false only after shutdown is complete.
+ * 
+ * Example:
+ * @code
+ * void longCallback()
+ * {
+ *   for (int i = 0; i < 1000000; i++)
+ *   {
+ *     if (robot::isShuttingDown())
+ *       return;  // Exit early
+ *     // Do work...
+ *   }
+ * }
+ * @endcode
+ */
+bool isShuttingDown();
+
+/**
+ * @brief Shutdown the robot system
+ * 
+ * Similar to ros::shutdown(), this function initiates the shutdown process.
+ * After calling this:
+ * - isShuttingDown() will return true immediately
+ * - ok() will return false after shutdown is complete
+ * 
+ * @note This function is thread-safe and can be called multiple times.
+ *       Subsequent calls have no effect.
+ * 
+ * Example:
+ * @code
+ * // Custom signal handler
+ * void mySigintHandler(int sig)
+ * {
+ *   // Do custom cleanup
+ *   robot::shutdown();
+ * }
+ * @endcode
+ */
+void shutdown();
+
+/**
+ * @brief Initialize the robot system
+ * 
+ * Similar to ros::init(), this function initializes the robot system.
+ * It sets up signal handlers and initializes internal state.
+ * 
+ * @param argc Number of command line arguments (can be modified)
+ * @param argv Command line arguments (can be modified)
+ * @param node_name Name of the node (optional, for logging purposes)
+ * @param install_sigint_handler If true, install SIGINT handler (default: true)
+ * 
+ * @note If install_sigint_handler is true, Ctrl-C will automatically
+ *       call shutdown(). Set to false if you want custom signal handling.
+ * 
+ * Example:
+ * @code
+ * int main(int argc, char** argv)
+ * {
+ *   robot::init(argc, argv, "my_node");
+ *   // ... use robot system ...
+ *   return 0;
+ * }
+ * @endcode
+ */
+void init(int& argc, char** argv, const std::string& node_name = "", bool install_sigint_handler = true);
+
+/**
+ * @brief Initialize the robot system without command line arguments
+ * 
+ * Simplified version of init() that doesn't require argc/argv.
+ * 
+ * @param node_name Name of the node (optional, for logging purposes)
+ * @param install_sigint_handler If true, install SIGINT handler (default: true)
+ */
+void init(const std::string& node_name = "", bool install_sigint_handler = true);
+
+namespace detail
+{
+  /**
+   * @brief Internal signal handler for SIGINT
+   * 
+   * This is called automatically when SIGINT is received (Ctrl-C).
+   * It calls shutdown() to gracefully shut down the system.
+   */
+  void sigintHandler(int sig);
+}
+
+} // namespace robot
+
+#endif // ROBOT_INIT_H_INCLUDED
+
--- a/src/Libraries/robot_cpp/package.xml
+++ b/src/Libraries/robot_cpp/package.xml
@ -2,12 +2,15 @@
  <name>robot_cpp</name>
  <version>0.7.10</version>
  <description>
-    robot_cpp is the second generation of the transform library, which lets
-    the user keep track of multiple coordinate frames over time. robot_cpp
-    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.
+    robot_cpp provides core utilities for robot application development, including:
+    - Console: Colored logging system with ANSI color support and throttle logging
+    - NodeHandle: ROS-like parameter management using YAML files as backend
+    - Init: System lifecycle management (init, ok, shutdown) with graceful shutdown handling
+    - PluginLoaderHelper: Helper to find library paths from symbol names for boost::dll plugins
+    
+    The library is designed to be independent of ROS and provides a foundation for
+    building robot applications with configuration management, logging, and plugin loading.
+    All operations are thread-safe and support both Catkin and Standalone CMake builds.
  </description>
  <author>Tully Foote</author>
  <author>Eitan Marder-Eppstein</author>
--- a/src/Libraries/robot_cpp/src/init.cpp
+++ b/src/Libraries/robot_cpp/src/init.cpp
@ -0,0 +1,139 @@
+#include <robot/init.h>
+#include <robot/console.h>
+#include <csignal>
+#include <atomic>
+#include <mutex>
+
+namespace robot
+{
+
+namespace detail
+{
+  // Internal state for shutdown management
+  std::atomic<bool> g_shutdown_requested(false);
+  std::atomic<bool> g_shutdown_complete(false);
+  std::atomic<bool> g_initialized(false);
+  std::mutex g_shutdown_mutex;
+  bool g_sigint_handler_installed = false;
+  
+  // Original SIGINT handler (if any)
+  void (*g_original_sigint_handler)(int) = nullptr;
+}
+
+void detail::sigintHandler(int sig)
+{
+  (void)sig; // Suppress unused parameter warning
+  
+  // Call shutdown
+  robot::shutdown();
+  
+  // If there was an original handler, call it too
+  if (detail::g_original_sigint_handler && 
+      detail::g_original_sigint_handler != SIG_DFL && 
+      detail::g_original_sigint_handler != SIG_IGN)
+  {
+    detail::g_original_sigint_handler(sig);
+  }
+}
+
+bool ok()
+{
+  // Return false if shutdown is complete
+  return !detail::g_shutdown_complete.load(std::memory_order_acquire);
+}
+
+bool isShuttingDown()
+{
+  // Return true if shutdown has been requested
+  return detail::g_shutdown_requested.load(std::memory_order_acquire);
+}
+
+void shutdown()
+{
+  std::lock_guard<std::mutex> lock(detail::g_shutdown_mutex);
+  
+  // If already shutting down, do nothing
+  if (detail::g_shutdown_requested.load(std::memory_order_acquire))
+  {
+    return;
+  }
+  
+  // Mark shutdown as requested
+  detail::g_shutdown_requested.store(true, std::memory_order_release);
+  
+  // Log shutdown
+  robot::log_info("Shutting down robot system...");
+  
+  // TODO: Add cleanup of resources here if needed
+  // For example:
+  // - Close all subscriptions/publications
+  // - Stop all timers
+  // - Cleanup threads
+  
+  // Mark shutdown as complete
+  // In a more complex system, you might want to wait for resources to cleanup
+  detail::g_shutdown_complete.store(true, std::memory_order_release);
+  
+  robot::log_info("Robot system shutdown complete");
+}
+
+void init(int& argc, char** argv, const std::string& node_name, bool install_sigint_handler)
+{
+  (void)argc;  // Suppress unused parameter warning
+  (void)argv;  // Suppress unused parameter warning
+  
+  std::lock_guard<std::mutex> lock(detail::g_shutdown_mutex);
+  
+  // If already initialized, do nothing
+  if (detail::g_initialized.load(std::memory_order_acquire))
+  {
+    robot::log_warning("robot::init() called multiple times, ignoring");
+    return;
+  }
+  
+  // Reset shutdown state
+  detail::g_shutdown_requested.store(false, std::memory_order_release);
+  detail::g_shutdown_complete.store(false, std::memory_order_release);
+  
+  // Install SIGINT handler if requested
+  if (install_sigint_handler)
+  {
+    // Save original handler
+    detail::g_original_sigint_handler = std::signal(SIGINT, detail::sigintHandler);
+    detail::g_sigint_handler_installed = true;
+    
+    if (!node_name.empty())
+    {
+      robot::log_info("Initialized robot system for node: %s", node_name.c_str());
+    }
+    else
+    {
+      robot::log_info("Initialized robot system");
+    }
+  }
+  else
+  {
+    if (!node_name.empty())
+    {
+      robot::log_info("Initialized robot system for node: %s (no SIGINT handler)", node_name.c_str());
+    }
+    else
+    {
+      robot::log_info("Initialized robot system (no SIGINT handler)");
+    }
+  }
+  
+  // Mark as initialized
+  detail::g_initialized.store(true, std::memory_order_release);
+}
+
+void init(const std::string& node_name, bool install_sigint_handler)
+{
+  // Dummy argc/argv for the other init() function
+  int dummy_argc = 0;
+  char** dummy_argv = nullptr;
+  init(dummy_argc, dummy_argv, node_name, install_sigint_handler);
+}
+
+} // namespace robot
+
--- a/src/Libraries/robot_time
+++ b/src/Libraries/robot_time
@ -1 +1 @@
-Subproject commit 6b23f32c6e3d3b969c840ef763c5fb22cc2e14d7
+Subproject commit 35b77e9fa25fe4473c9fb0215f902ea587347859
--- 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,10 +477,9 @@ 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.
@ -471,16 +487,31 @@ namespace move_base_core
 		 */
 		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