diff --git a/CMakeLists.txt b/CMakeLists.txt
index 10dc4bd..b29e3d7 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -150,6 +150,9 @@ endif()
 if (NOT TARGET laser_geometry)
     add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../laser_geometry ${CMAKE_BINARY_DIR}/laser_geometry_build)
 endif()
+if (NOT TARGET voxel_grid)
+    add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../voxel_grid ${CMAKE_BINARY_DIR}/voxel_grid_build)
+endif()
 
 include_directories(
     include  
@@ -183,6 +186,7 @@ target_link_libraries(costmap_2d_new
     nav_msgs
     map_msgs
     laser_geometry
+    voxel_grid
     ${TF2_LIBRARY}
 )
 target_include_directories(costmap_2d_new PRIVATE ${Boost_INCLUDE_DIRS})
diff --git a/CODE_REVIEW.md b/CODE_REVIEW.md
new file mode 100644
index 0000000..ff15cca
--- /dev/null
+++ b/CODE_REVIEW.md
@@ -0,0 +1,440 @@
+# Code Review: costmap_2d Package
+## Các vấn đề có thể gây Core Dump và Nhầm lẫn
+
+---
+
+## 🔴 **VẤN ĐỀ NGHIÊM TRỌNG - CÓ THỂ GÂY CORE DUMP**
+
+### 1. **Thiếu kiểm tra biên (Bounds Checking) trong `getCost()` và `setCost()`**
+
+**File:** `src/costmap_2d.cpp:195-206`
+
+```cpp
+unsigned char Costmap2D::getCost(unsigned int mx, unsigned int my) const
+{
+  return costmap_[getIndex(mx, my)];  // ❌ KHÔNG KIỂM TRA mx < size_x_ || my < size_y_
+}
+
+void Costmap2D::setCost(unsigned int mx, unsigned int my, unsigned char cost)
+{
+  costmap_[getIndex(mx, my)] = cost;  // ❌ KHÔNG KIỂM TRA mx < size_x_ || my < size_y_
+}
+```
+
+**Vấn đề:**
+- Nếu `mx >= size_x_` hoặc `my >= size_y_`, `getIndex()` sẽ tính sai và truy cập ngoài mảng
+- Có thể gây **segmentation fault** hoặc **buffer overflow**
+
+**Giải pháp:**
+```cpp
+unsigned char Costmap2D::getCost(unsigned int mx, unsigned int my) const
+{
+  if (mx >= size_x_ || my >= size_y_ || costmap_ == nullptr)
+    return default_value_;  // hoặc throw exception
+  return costmap_[getIndex(mx, my)];
+}
+```
+
+---
+
+### 2. **Lỗi logic nghiêm trọng trong `updateOrigin()` - Có thể gây Core Dump**
+
+**File:** `src/costmap_2d.cpp:264-305`
+
+**Vấn đề 1: Tính toán bounds sai:**
+```cpp
+int lower_left_x = min(max(cell_ox, 0), size_x);  // ❌ SAI LOGIC
+int lower_left_y = min(max(cell_oy, 0), size_y);
+int upper_right_x = min(max(cell_ox + size_x, 0), size_x);  // ❌ SAI LOGIC
+int upper_right_y = min(max(cell_oy + size_y, 0), size_y);
+```
+
+**Giải thích:**
+- `max(cell_ox + size_x, 0)` luôn >= 0, nên `min(..., size_x)` sẽ luôn trả về `size_x` nếu `cell_ox + size_x > size_x`
+- Điều này có thể dẫn đến `cell_size_x = 0` hoặc giá trị âm (unsigned int wrap-around)
+
+**Vấn đề 2: Không kiểm tra cell_size trước khi cấp phát:**
+```cpp
+unsigned int cell_size_x = upper_right_x - lower_left_x;  // Có thể = 0 hoặc âm (wrap-around)
+unsigned int cell_size_y = upper_right_y - lower_left_y;
+unsigned char* local_map = new unsigned char[cell_size_x * cell_size_y];  // ❌ NGUY HIỂM
+```
+
+**Vấn đề 3: Truy cập mảng không an toàn trong `copyMapRegion()`:**
+- Nếu `start_x < 0` hoặc `start_y < 0`, sẽ truy cập ngoài mảng
+- Nếu `start_x + cell_size_x > size_x_`, sẽ vượt quá biên
+
+**Giải pháp đề xuất:**
+```cpp
+void Costmap2D::updateOrigin(double new_origin_x, double new_origin_y)
+{
+  int cell_ox = int((new_origin_x - origin_x_) / resolution_);
+  int cell_oy = int((new_origin_y - origin_y_) / resolution_);
+
+  if (cell_ox == 0 && cell_oy == 0)
+    return;
+
+  double new_grid_ox = origin_x_ + cell_ox * resolution_;
+  double new_grid_oy = origin_y_ + cell_oy * resolution_;
+
+  // Tính vùng overlap đúng cách
+  int src_min_x = std::max(0, cell_ox);
+  int src_min_y = std::max(0, cell_oy);
+  int src_max_x = std::min(int(size_x_), cell_ox + int(size_x_));
+  int src_max_y = std::min(int(size_y_), cell_oy + int(size_y_));
+
+  // Kiểm tra có overlap không
+  if (src_max_x <= src_min_x || src_max_y <= src_min_y) {
+    // Không có overlap, chỉ cần reset
+    origin_x_ = new_grid_ox;
+    origin_y_ = new_grid_oy;
+    resetMaps();
+    return;
+  }
+
+  unsigned int cell_size_x = src_max_x - src_min_x;
+  unsigned int cell_size_y = src_max_y - src_min_y;
+
+  unsigned char* local_map = new unsigned char[cell_size_x * cell_size_y];
+  
+  // Copy vùng overlap
+  copyMapRegion(costmap_, src_min_x, src_min_y, size_x_,
+                local_map, 0, 0, cell_size_x, cell_size_x, cell_size_y);
+
+  resetMaps();
+  origin_x_ = new_grid_ox;
+  origin_y_ = new_grid_oy;
+
+  // Tính vị trí đích trong map mới
+  int dst_min_x = src_min_x - cell_ox;
+  int dst_min_y = src_min_y - cell_oy;
+
+  // Kiểm tra bounds trước khi copy
+  if (dst_min_x >= 0 && dst_min_y >= 0 && 
+      dst_min_x + int(cell_size_x) <= int(size_x_) &&
+      dst_min_y + int(cell_size_y) <= int(size_y_)) {
+    copyMapRegion(local_map, 0, 0, cell_size_x,
+                  costmap_, dst_min_x, dst_min_y, size_x_, cell_size_x, cell_size_y);
+  }
+
+  delete[] local_map;
+}
+```
+
+---
+
+### 3. **Lỗi trong `resetMap()` - Có thể gây truy cập ngoài mảng**
+
+**File:** `src/costmap_2d.cpp:84-90`
+
+```cpp
+void Costmap2D::resetMap(unsigned int x0, unsigned int y0, unsigned int xn, unsigned int yn)
+{
+  boost::unique_lock<mutex_t> lock(*(access_));
+  unsigned int len = xn - x0;
+  for (unsigned int y = y0 * size_x_ + x0; y < yn * size_x_ + x0; y += size_x_)  // ❌ NGUY HIỂM
+    memset(costmap_ + y, default_value_, len * sizeof(unsigned char));
+}
+```
+
+**Vấn đề:**
+- Không kiểm tra `x0 < xn`, `y0 < yn`
+- Không kiểm tra `xn <= size_x_`, `yn <= size_y_`
+- Nếu `y0 * size_x_ + x0` vượt quá kích thước mảng, `memset()` sẽ ghi đè bộ nhớ ngoài mảng
+
+**Giải pháp:**
+```cpp
+void Costmap2D::resetMap(unsigned int x0, unsigned int y0, unsigned int xn, unsigned int yn)
+{
+  boost::unique_lock<mutex_t> lock(*(access_));
+  
+  // Kiểm tra bounds
+  if (x0 >= size_x_ || y0 >= size_y_ || xn > size_x_ || yn > size_y_ || 
+      x0 >= xn || y0 >= yn || costmap_ == nullptr)
+    return;  // hoặc throw exception
+
+  unsigned int len = xn - x0;
+  for (unsigned int y = y0; y < yn; y++) {
+    unsigned int offset = y * size_x_ + x0;
+    memset(costmap_ + offset, default_value_, len * sizeof(unsigned char));
+  }
+}
+```
+
+---
+
+### 4. **Thiếu kiểm tra NULL pointer trong `getCharMap()`**
+
+**File:** `src/costmap_2d.cpp:187-190`
+
+```cpp
+unsigned char* Costmap2D::getCharMap() const
+{
+  return costmap_;  // ❌ Có thể trả về NULL nếu chưa khởi tạo
+}
+```
+
+**Vấn đề:**
+- Nếu `costmap_` là `NULL`, các hàm khác sử dụng `getCharMap()` sẽ crash
+- Đặc biệt nguy hiểm trong `costmap_layer.cpp` khi truy cập `master_array[it]`
+
+**Giải pháp:**
+```cpp
+unsigned char* Costmap2D::getCharMap() const
+{
+  if (costmap_ == nullptr) {
+    throw std::runtime_error("Costmap2D::getCharMap() called before initialization");
+  }
+  return costmap_;
+}
+```
+
+---
+
+### 5. **Race condition trong `initMaps()` và `deleteMaps()`**
+
+**File:** `src/costmap_2d.cpp:36-53`
+
+**Vấn đề:**
+- `initMaps()` lock mutex nhưng `deleteMaps()` cũng lock
+- Nếu gọi đồng thời từ nhiều thread, có thể gây deadlock hoặc double delete
+
+**Giải pháp:**
+- Đảm bảo tất cả truy cập đều lock mutex
+- Sử dụng `std::lock_guard` thay vì `unique_lock` nếu không cần unlock sớm
+
+---
+
+## 🟡 **VẤN ĐỀ CÓ THỂ GÂY NHẦM LẪN**
+
+### 6. **Thiếu kiểm tra bounds trong các hàm update của CostmapLayer**
+
+**File:** `src/costmap_layer.cpp:63-101`
+
+```cpp
+void CostmapLayer::updateWithMax(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
+{
+  // ...
+  for (int j = min_j; j < max_j; j++)
+  { 
+    unsigned int it = j * span + min_i;  // ❌ Không kiểm tra j >= 0, j < size_y
+    for (int i = min_i; i < max_i; i++)
+    { 
+      if (costmap_[it] == NO_INFORMATION){  // ❌ it có thể vượt quá bounds
+        // ...
+      }
+      // ...
+    }
+  }
+}
+```
+
+**Vấn đề:**
+- Không kiểm tra `min_i`, `min_j`, `max_i`, `max_j` có hợp lệ không
+- `it` có thể vượt quá kích thước mảng nếu `min_i < 0` hoặc `j < 0`
+
+**Giải pháp:**
+```cpp
+void CostmapLayer::updateWithMax(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
+{
+  if (!enabled_)
+    return;
+
+  // Kiểm tra bounds
+  int size_x = getSizeInCellsX();
+  int size_y = getSizeInCellsY();
+  min_i = std::max(0, min_i);
+  min_j = std::max(0, min_j);
+  max_i = std::min(int(size_x), max_i);
+  max_j = std::min(int(size_y), max_j);
+
+  if (min_i >= max_i || min_j >= max_j)
+    return;
+
+  // ... phần còn lại
+}
+```
+
+---
+
+### 7. **Lỗi trong `clearArea()` - Logic điều kiện phức tạp**
+
+**File:** `src/costmap_layer.cpp:21-36`
+
+```cpp
+void CostmapLayer::clearArea(int start_x, int start_y, int end_x, int end_y, bool invert_area)
+{
+  unsigned char* grid = getCharMap();
+  for(int x=0; x<(int)getSizeInCellsX(); x++){
+    bool xrange = x>start_x && x<end_x;  // ❌ Sử dụng > và < thay vì >= và <=
+
+    for(int y=0; y<(int)getSizeInCellsY(); y++){
+      if((xrange && y>start_y && y<end_y)!=invert_area)  // ❌ Logic phức tạp, dễ nhầm
+        continue;
+      // ...
+    }
+  }
+}
+```
+
+**Vấn đề:**
+- Logic điều kiện `(xrange && y>start_y && y<end_y)!=invert_area` rất khó hiểu
+- Không kiểm tra `start_x < end_x`, `start_y < end_y`
+- Sử dụng `>` và `<` thay vì `>=` và `<=` có thể bỏ sót các ô ở biên
+
+---
+
+### 8. **Thiếu kiểm tra trong `worldToMap()` - Có thể trả về giá trị sai**
+
+**File:** `src/costmap_2d.cpp:220-229`
+
+```cpp
+bool Costmap2D::worldToMap(double wx, double wy, unsigned int& mx, unsigned int& my) const
+{
+  if (wx < origin_x_ || wy < origin_y_)
+    return false;
+
+  mx = (int)((wx - origin_x_) / resolution_);  // ❌ Có thể âm nếu wx < origin_x_
+  my = (int)((wy - origin_y_) / resolution_);    // ❌ Có thể âm nếu wy < origin_y_
+
+  return (mx < size_x_ && my < size_y_);  // ❌ Không kiểm tra mx >= 0, my >= 0
+}
+```
+
+**Vấn đề:**
+- Nếu `wx < origin_x_`, hàm trả về `false` nhưng `mx` đã được gán giá trị (có thể âm)
+- Nếu `mx` hoặc `my` âm, so sánh `mx < size_x_` vẫn có thể true (unsigned int comparison)
+
+**Giải pháp:**
+```cpp
+bool Costmap2D::worldToMap(double wx, double wy, unsigned int& mx, unsigned int& my) const
+{
+  if (wx < origin_x_ || wy < origin_y_)
+    return false;
+
+  int mx_int = (int)((wx - origin_x_) / resolution_);
+  int my_int = (int)((wy - origin_y_) / resolution_);
+
+  if (mx_int < 0 || my_int < 0)
+    return false;
+
+  mx = (unsigned int)mx_int;
+  my = (unsigned int)my_int;
+
+  return (mx < size_x_ && my < size_y_);
+}
+```
+
+---
+
+## 🟠 **VẤN ĐỀ TRONG CMAKELISTS.TXT**
+
+### 9. **Hardcoded path đến tf2 library**
+
+**File:** `CMakeLists.txt:135`
+
+```cmake
+set(TF2_LIBRARY /usr/lib/libtf2.so)  # ❌ Hardcoded path
+```
+
+**Vấn đề:**
+- Không portable, sẽ fail trên các hệ thống khác
+- Nên dùng `find_package(tf2)` hoặc `find_library()`
+
+**Giải pháp:**
+```cmake
+find_package(tf2 REQUIRED)
+# hoặc
+find_library(TF2_LIBRARY tf2 PATHS /usr/lib /usr/local/lib)
+```
+
+---
+
+### 10. **RPATH settings có thể không hoạt động đúng**
+
+**File:** `CMakeLists.txt:122-127`
+
+```cmake
+set(CMAKE_BUILD_RPATH "${CMAKE_BINARY_DIR}/costmap_2d")  # ❌ Có thể sai
+set(CMAKE_INSTALL_RPATH "${CMAKE_BINARY_DIR}/costmap_2d")
+```
+
+**Vấn đề:**
+- `CMAKE_BINARY_DIR` là thư mục build gốc, không phải thư mục build của costmap_2d
+- Nên dùng `${CMAKE_CURRENT_BINARY_DIR}` hoặc `${CMAKE_BINARY_DIR}/costmap_2d`
+
+---
+
+### 11. **Thiếu PUBLIC/PRIVATE trong target_include_directories**
+
+**File:** `CMakeLists.txt:188`
+
+```cmake
+target_include_directories(costmap_2d_new PRIVATE ${Boost_INCLUDE_DIRS})  # ❌ Chỉ PRIVATE
+```
+
+**Vấn đề:**
+- Nếu các target khác link với `costmap_2d_new`, họ cần Boost headers nhưng không nhận được
+- Nên thêm `PUBLIC` hoặc `INTERFACE`
+
+---
+
+## 🔵 **VẤN ĐỀ TIỀM ẨN KHÁC**
+
+### 12. **Memory leak tiềm ẩn trong `updateOrigin()`**
+
+Nếu exception xảy ra giữa `new[]` và `delete[]`, sẽ bị memory leak. Nên dùng smart pointer:
+
+```cpp
+std::unique_ptr<unsigned char[]> local_map(new unsigned char[cell_size_x * cell_size_y]);
+```
+
+---
+
+### 13. **Thread safety trong `getCost()` và `setCost()`**
+
+Các hàm này không lock mutex, nhưng có thể được gọi từ nhiều thread. Nên thêm lock:
+
+```cpp
+unsigned char Costmap2D::getCost(unsigned int mx, unsigned int my) const
+{
+  boost::unique_lock<mutex_t> lock(*access_);
+  // ... kiểm tra bounds và trả về
+}
+```
+
+---
+
+### 14. **ObservationBuffer - Thiếu kiểm tra null pointer**
+
+**File:** `src/observation_buffer.cpp:204`
+
+```cpp
+if ((last_updated_ - obs.cloud_->header.stamp) > observation_keep_time_)  // ❌ obs.cloud_ có thể NULL
+```
+
+**Vấn đề:**
+- `obs.cloud_` là shared_ptr, nhưng nên kiểm tra trước khi truy cập
+
+---
+
+## 📋 **TÓM TẮT CÁC VẤN ĐỀ NGHIÊM TRỌNG NHẤT**
+
+1. ✅ **Thiếu bounds checking** trong `getCost()`, `setCost()`, `getIndex()` → **CORE DUMP**
+2. ✅ **Lỗi logic nghiêm trọng** trong `updateOrigin()` → **CORE DUMP**
+3. ✅ **Thiếu kiểm tra bounds** trong `resetMap()` → **CORE DUMP**
+4. ✅ **NULL pointer** trong `getCharMap()` → **CORE DUMP**
+5. ✅ **Thiếu bounds checking** trong các hàm update của CostmapLayer → **CORE DUMP**
+6. ✅ **Hardcoded paths** trong CMakeLists.txt → **Build failure trên hệ thống khác**
+
+---
+
+## 🛠️ **KHUYẾN NGHỊ**
+
+1. **Thêm assertions hoặc exceptions** cho tất cả các hàm truy cập mảng
+2. **Sử dụng AddressSanitizer (ASan)** để phát hiện memory errors
+3. **Thêm unit tests** cho các edge cases (bounds, NULL pointers, etc.)
+4. **Sử dụng smart pointers** thay vì raw pointers
+5. **Thêm logging** để debug các vấn đề runtime
+6. **Review lại toàn bộ logic** trong `updateOrigin()` và `resetMap()`
+
diff --git a/include/costmap_2d/costmap_2d_robot.h b/include/costmap_2d/costmap_2d_robot.h
index e7d7b27..bfd1cb8 100644
--- a/include/costmap_2d/costmap_2d_robot.h
+++ b/include/costmap_2d/costmap_2d_robot.h
@@ -47,7 +47,7 @@
 #include <geometry_msgs/PolygonStamped.h>
 #include <geometry_msgs/PoseStamped.h>
 #include <dynamic_reconfigure/server.h>
-#include <pluginlib/class_loader.hpp>
+// #include <pluginlib/class_loader.hpp>
 #include <tf2/LinearMath/Transform.h>
 
 // class SuperValue : public XmlRpc::XmlRpcValue
@@ -71,7 +71,7 @@ namespace costmap_2d
 /** @brief A ROS wrapper for a 2D Costmap. Handles subscribing to
  * topics that provide observations about obstacles in either the form
  * of PointCloud or LaserScan messages. */
-class Costmap2DRobot
+class Costmap2DROS
 {
 public:
   /**
@@ -79,8 +79,8 @@ public:
    * @param name The name for this costmap
    * @param tf A reference to a TransformListener
    */
-  Costmap2DRobot(const std::string &name, tf2::BufferCore& tf);
-  ~Costmap2DRobot();
+  Costmap2DROS(const std::string &name, tf2_ros::Buffer& tf);
+  ~Costmap2DROS();
 
   /**
    * @brief  Subscribes to sensor topics if necessary and starts costmap
@@ -255,28 +255,28 @@ private:
   void warnForOldParameters(ros::NodeHandle& nh);
   void checkOldParam(ros::NodeHandle& nh, const std::string &param_name);
   void copyParentParameters(const std::string& plugin_name, const std::string& plugin_type, ros::NodeHandle& nh);
-  // void reconfigureCB(costmap_2d::Costmap2DConfig &config, uint32_t level);
+  void reconfigureCB(costmap_2d::Costmap2DConfig &config, uint32_t level);
   void movementCB(const ros::TimerEvent &event);
   void mapUpdateLoop(double frequency);
   bool map_update_thread_shutdown_;
   bool stop_updates_, initialized_, stopped_;
   boost::thread* map_update_thread_;  ///< @brief A thread for updating the map
-  // ros::Time last_publish_;
-  // ros::Duration publish_cycle;
-  // pluginlib::ClassLoader<costmap_2d::Layer> plugin_loader_;
-  // Costmap2DPublisher* publisher_;
-  // dynamic_reconfigure::Server<costmap_2d::Costmap2DConfig> *dsrv_;
+  ros::Time last_publish_;
+  ros::Duration publish_cycle;
+  pluginlib::ClassLoader<costmap_2d::Layer> plugin_loader_;
+  Costmap2DPublisher* publisher_;
+  dynamic_reconfigure::Server<costmap_2d::Costmap2DConfig> *dsrv_;
 
   boost::recursive_mutex configuration_mutex_;
 
-  // ros::Subscriber footprint_sub_;
-  // ros::Publisher footprint_pub_;
+  ros::Subscriber footprint_sub_;
+  ros::Publisher footprint_pub_;
   std::vector<geometry_msgs::Point> unpadded_footprint_;
   std::vector<geometry_msgs::Point> padded_footprint_;
   float footprint_padding_;
-  // costmap_2d::Costmap2DConfig old_config_;
+  costmap_2d::Costmap2DConfig old_config_;
 };
-// class Costmap2DRobot
+// class Costmap2DROS
 }  // namespace costmap_2d
 
 #endif  // COSTMAP_2D_COSTMAP_2D_ROS_H
diff --git a/include/costmap_2d/costmap_layer.h b/include/costmap_2d/costmap_layer.h
index f327fe7..5e6f4d6 100644
--- a/include/costmap_2d/costmap_layer.h
+++ b/include/costmap_2d/costmap_layer.h
@@ -71,22 +71,7 @@ public:
    */
   void addExtraBounds(double mx0, double my0, double mx1, double my1);
 
-  template<typename T>
-  void dataCallBack(const T& value, const std::string& topic) {
-      handle(value, topic);
-  }
-
 protected:
-  // Hàm template public, dùng để gửi dữ liệu
-  template<typename T>
-  void handle(const T& data, const std::string& topic) {
-      handleImpl(static_cast<const void*>(&data), typeid(T), topic);
-  }
-
-  // Hàm ảo duy nhất mà plugin sẽ override
-  virtual void handleImpl(const void* data, 
-                          const std::type_info& type, 
-                          const std::string& topic) = 0;
   /*
    * Updates the master_grid within the specified
    * bounding box using this layer's values.
@@ -162,7 +147,5 @@ private:
   double extra_min_x_, extra_max_x_, extra_min_y_, extra_max_y_;
 };
 
-using PluginCostmapLayerPtr = std::shared_ptr<CostmapLayer>;
-
 }  // namespace costmap_2d
 #endif  // COSTMAP_2D_COSTMAP_LAYER_H_
diff --git a/include/costmap_2d/inflation_layer.h b/include/costmap_2d/inflation_layer.h
index d1d21d7..2b49135 100644
--- a/include/costmap_2d/inflation_layer.h
+++ b/include/costmap_2d/inflation_layer.h
@@ -43,9 +43,16 @@ public:
 
   virtual ~InflationLayer()
   {
+    // ✅ FIX: Delete mutex được tạo bằng new (phải delete trước để tránh lock trong destructor)
+    if (inflation_access_)
+        delete inflation_access_;
+    inflation_access_ = nullptr;
+    
+    // Delete kernels và seen array
     deleteKernels();
     if (seen_)
         delete[] seen_;
+    seen_ = nullptr;
   }
 
   virtual void onInitialize();
@@ -98,6 +105,9 @@ protected:
   bool inflate_unknown_;
 
 private:
+  void handleImpl(const void* data,
+                              const std::type_info& info,
+                              const std::string& source) override;
   /**
    * @brief  Lookup pre-computed distances
    * @param mx The x coordinate of the current cell
diff --git a/include/costmap_2d/layer.h b/include/costmap_2d/layer.h
index 5ad9c00..35b6c81 100644
--- a/include/costmap_2d/layer.h
+++ b/include/costmap_2d/layer.h
@@ -41,6 +41,8 @@
 #include <costmap_2d/layered_costmap.h>
 #include <string>
 #include <tf2/buffer_core.h>
+#include <costmap_2d/layered_costmap.h>
+#include <boost/shared_ptr.hpp>
 
 namespace costmap_2d
 {
@@ -129,7 +131,22 @@ public:
    * notified of changes to the robot's footprint. */
   virtual void onFootprintChanged() {}
 
+  template<typename T>
+  void dataCallBack(const T& value, const std::string& topic) {
+      handle(value, topic);
+  }
+
 protected:
+  // Hàm template public, dùng để gửi dữ liệu
+  template<typename T>
+  void handle(const T& data, const std::string& topic) {
+      handleImpl(static_cast<const void*>(&data), typeid(T), topic);
+  }
+
+  // Hàm ảo duy nhất mà plugin sẽ override
+  virtual void handleImpl(const void* data, 
+                          const std::type_info& type, 
+                          const std::string& topic) = 0;
   /** @brief This is called at the end of initialize().  Override to
    * implement subclass-specific initialization.
    *
@@ -146,8 +163,6 @@ private:
   std::vector<geometry_msgs::Point> footprint_spec_;
 };
 
-using PluginLayerPtr = std::shared_ptr<Layer>;
-
 }  // namespace costmap_2d
 
 #endif  // COSTMAP_2D_LAYER_H_
diff --git a/include/costmap_2d/layered_costmap.h b/include/costmap_2d/layered_costmap.h
index 1e8a0eb..f04b50d 100644
--- a/include/costmap_2d/layered_costmap.h
+++ b/include/costmap_2d/layered_costmap.h
@@ -1,17 +1,19 @@
 #ifndef COSTMAP_2D_LAYERED_COSTMAP_H_
 #define COSTMAP_2D_LAYERED_COSTMAP_H_
 
-#include <costmap_2d/cost_values.h>   // Chứa các hằng số giá trị chi phí (ví dụ: FREE_SPACE, LETHAL_OBSTACLE, NO_INFORMATION)
 #include <costmap_2d/layer.h>         // Lớp cơ sở cho các lớp (layer) con: static layer, obstacle layer, inflation layer...
+#include <costmap_2d/cost_values.h>   // Chứa các hằng số giá trị chi phí (ví dụ: FREE_SPACE, LETHAL_OBSTACLE, NO_INFORMATION)
 #include <costmap_2d/costmap_2d.h>    // Định nghĩa lớp Costmap2D (bản đồ 2D cơ bản)
 #include <vector>
 #include <string>
+#include <boost/dll/import.hpp>
+
 
 namespace costmap_2d
 {
 
 class Layer;  // Khai báo trước để dùng trong vector plugin
-
+using PluginLayerPtr = std::shared_ptr<Layer>;
 /**
  * @class LayeredCostmap
  * @brief Lớp này quản lý nhiều "layer" (tầng bản đồ) khác nhau và kết hợp chúng thành một bản đồ chi phí tổng hợp.
@@ -102,7 +104,7 @@ public:
    * @brief Trả về danh sách các plugin (layer) đã được nạp.
    * @return vector các shared_ptr<Layer>.
    */
-  std::vector<boost::shared_ptr<Layer> >* getPlugins()
+  std::vector<costmap_2d::PluginLayerPtr>* getPlugins()
   {
     return &plugins_;
   }
@@ -111,7 +113,7 @@ public:
    * @brief Thêm một plugin (layer) mới vào bản đồ.
    * @param plugin: con trỏ thông minh đến lớp layer.
    */
-  void addPlugin(boost::shared_ptr<Layer> plugin)
+  void addPlugin(costmap_2d::PluginLayerPtr plugin)
   {
     plugins_.push_back(plugin);
   }
@@ -179,7 +181,7 @@ private:
   double minx_, miny_, maxx_, maxy_; ///< Giới hạn vùng bản đồ được cập nhật (tọa độ thế giới)
   unsigned int bx0_, bxn_, by0_, byn_; ///< Giới hạn vùng bản đồ được cập nhật (tọa độ lưới)
 
-  std::vector<boost::shared_ptr<Layer> > plugins_; ///< Danh sách các layer plugin (ví dụ: StaticLayer, ObstacleLayer, InflationLayer)
+  std::vector<costmap_2d::PluginLayerPtr> plugins_; ///< Danh sách các layer plugin (ví dụ: StaticLayer, ObstacleLayer, InflationLayer)
 
   bool initialized_; ///< Đã được khởi tạo hoàn toàn hay chưa
   bool size_locked_; ///< Có cho phép thay đổi kích thước bản đồ sau này không
diff --git a/include/costmap_2d/obstacle_layer.h b/include/costmap_2d/obstacle_layer.h
index e1abc8e..bf813d9 100644
--- a/include/costmap_2d/obstacle_layer.h
+++ b/include/costmap_2d/obstacle_layer.h
@@ -149,8 +149,6 @@ private:
   bool getParams();
 };
 
-using PluginObstacleLayerPtr = std::shared_ptr<ObstacleLayer>;
-
 }  // namespace costmap_2d
 
 #endif  // COSTMAP_2D_OBSTACLE_LAYER_H_
diff --git a/include/costmap_2d/static_layer.h b/include/costmap_2d/static_layer.h
index ff3640f..c70be56 100644
--- a/include/costmap_2d/static_layer.h
+++ b/include/costmap_2d/static_layer.h
@@ -64,8 +64,6 @@ private:
   bool getParams();
 };
 
-using PluginStaticLayerPtr = std::shared_ptr<StaticLayer>;
-
 }  // namespace costmap_2d
 
 #endif  // COSTMAP_2D_STATIC_LAYER_H_
diff --git a/include/costmap_2d/voxel_layer.h b/include/costmap_2d/voxel_layer.h
index e12c436..7b4fc66 100644
--- a/include/costmap_2d/voxel_layer.h
+++ b/include/costmap_2d/voxel_layer.h
@@ -53,7 +53,7 @@
 // #include <dynamic_reconfigure/server.h>
 // #include <costmap_2d/VoxelPluginConfig.h>
 #include <costmap_2d/obstacle_layer.h>
-// #include <voxel_grid/voxel_grid.h>
+#include <voxel_grid/voxel_grid.h>
 
 namespace costmap_2d
 {
diff --git a/plugins/critical_layer.cpp b/plugins/critical_layer.cpp
index 22e1554..005d646 100644
--- a/plugins/critical_layer.cpp
+++ b/plugins/critical_layer.cpp
@@ -36,12 +36,12 @@ void CriticalLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, i
 }
 
 // Export factory function
-static PluginStaticLayerPtr create_critical_plugin() {
+static PluginLayerPtr create_critical_plugin() {
     return std::make_shared<CriticalLayer>();
 }
 
 // Alias cho Boost.DLL (nếu muốn dùng boost::dll::import_alias)
-BOOST_DLL_ALIAS(create_critical_plugin, create_plugin_static_layer)
+BOOST_DLL_ALIAS(create_critical_plugin, create_plugin)
 
 
 }
\ No newline at end of file
diff --git a/plugins/directional_layer.cpp b/plugins/directional_layer.cpp
index 6230b0b..2e22c71 100644
--- a/plugins/directional_layer.cpp
+++ b/plugins/directional_layer.cpp
@@ -429,10 +429,10 @@ namespace costmap_2d
   }
 
   // Export factory function
-  static PluginStaticLayerPtr create_directional_plugin() {
+  static PluginLayerPtr create_directional_plugin() {
       return std::make_shared<DirectionalLayer>();
   }
 
   // Alias cho Boost.DLL (nếu muốn dùng boost::dll::import_alias)
-  BOOST_DLL_ALIAS(create_directional_plugin, create_plugin_static_layer)
+  BOOST_DLL_ALIAS(create_directional_plugin, create_plugin)
 }
\ No newline at end of file
diff --git a/plugins/inflation_layer.cpp b/plugins/inflation_layer.cpp
index a7a48ae..c83320d 100644
--- a/plugins/inflation_layer.cpp
+++ b/plugins/inflation_layer.cpp
@@ -47,7 +47,7 @@ void InflationLayer::onInitialize()
     getParams();
   }
 
-  matchSize();
+  InflationLayer::matchSize();
 }
 
 bool InflationLayer::getParams()
@@ -350,12 +350,19 @@ void InflationLayer::setInflationParameters(double inflation_radius, double cost
   }
 }
 
+void InflationLayer::handleImpl(const void* data,
+                              const std::type_info& info,
+                              const std::string& source)
+{
+  printf("This function is not available!\n");
+}
+
 // Export factory function
 static PluginLayerPtr create_inflation_plugin() {
     return std::make_shared<InflationLayer>();
 }
 
 // Alias cho Boost.DLL (nếu muốn dùng boost::dll::import_alias)
-BOOST_DLL_ALIAS(create_inflation_plugin, create_plugin_layer)
+BOOST_DLL_ALIAS(create_inflation_plugin, create_plugin)
 
 }  // namespace costmap_2d
diff --git a/plugins/obstacle_layer.cpp b/plugins/obstacle_layer.cpp
index 306ea49..cf72446 100644
--- a/plugins/obstacle_layer.cpp
+++ b/plugins/obstacle_layer.cpp
@@ -19,7 +19,6 @@ namespace costmap_2d
 void ObstacleLayer::onInitialize()
 {
   rolling_window_ = layered_costmap_->isRolling();
-  
 
   ObstacleLayer::matchSize();
   current_ = true;
@@ -103,7 +102,7 @@ bool ObstacleLayer::getParams()
 
       printf(
           "Created an observation buffer for topic %s, global frame: %s, "
-          "expected update rate: %.2f, observation persistence: %.2f",
+          "expected update rate: %.2f, observation persistence: %.2f\n",
           topic.c_str(), global_frame_.c_str(), expected_update_rate, observation_keep_time);
 
     } 
@@ -517,7 +516,7 @@ void ObstacleLayer::reset()
 }
 
 // Export factory function
-static PluginCostmapLayerPtr create_obstacle_plugin() {
+static PluginLayerPtr create_obstacle_plugin() {
     return std::make_shared<ObstacleLayer>();
 }
 
diff --git a/plugins/preferred_layer.cpp b/plugins/preferred_layer.cpp
index dec8347..19bfb79 100644
--- a/plugins/preferred_layer.cpp
+++ b/plugins/preferred_layer.cpp
@@ -25,11 +25,11 @@ unsigned char PreferredLayer::interpretValue(unsigned char value)
 }
 
 // Export factory function
-static PluginStaticLayerPtr create_preferred_plugin() {
+static PluginLayerPtr create_preferred_plugin() {
     return std::make_shared<PreferredLayer>();
 }
 
 // Alias cho Boost.DLL (nếu muốn dùng boost::dll::import_alias)
-BOOST_DLL_ALIAS(create_preferred_plugin, create_plugin_static_layer)
+BOOST_DLL_ALIAS(create_preferred_plugin, create_plugin)
 
 }
\ No newline at end of file
diff --git a/plugins/static_layer.cpp b/plugins/static_layer.cpp
index c12aee4..7c19373 100644
--- a/plugins/static_layer.cpp
+++ b/plugins/static_layer.cpp
@@ -325,7 +325,7 @@ void StaticLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int
 }
 
 // Export factory function
-static PluginCostmapLayerPtr create_static_plugin() {
+static PluginLayerPtr create_static_plugin() {
     return std::make_shared<StaticLayer>();
 }
 
diff --git a/plugins/unpreferred_layer.cpp b/plugins/unpreferred_layer.cpp
index 484e8f4..1cb861a 100644
--- a/plugins/unpreferred_layer.cpp
+++ b/plugins/unpreferred_layer.cpp
@@ -23,11 +23,11 @@ unsigned char UnPreferredLayer::interpretValue(unsigned char value)
 }
 
 // Export factory function
-static PluginStaticLayerPtr create_unpreferred_plugin() {
+static PluginLayerPtr create_unpreferred_plugin() {
     return std::make_shared<UnPreferredLayer>();
 }
 
 // Alias cho Boost.DLL (nếu muốn dùng boost::dll::import_alias)
-BOOST_DLL_ALIAS(create_unpreferred_plugin, create_plugin_static_layer)
+BOOST_DLL_ALIAS(create_unpreferred_plugin, create_plugin)
 
 }
\ No newline at end of file
diff --git a/plugins/voxel_layer.cpp b/plugins/voxel_layer.cpp
index 6afe05b..ae2c9b2 100644
--- a/plugins/voxel_layer.cpp
+++ b/plugins/voxel_layer.cpp
@@ -445,11 +445,11 @@ void VoxelLayer::matchSize()
 // }
 
 // Export factory function
-static PluginObstacleLayerPtr create_voxel_plugin() {
+static PluginLayerPtr create_voxel_plugin() {
     return std::make_shared<VoxelLayer>();
 }
 
 // Alias cho Boost.DLL (nếu muốn dùng boost::dll::import_alias)
-BOOST_DLL_ALIAS(create_voxel_plugin, create_obstacle_plugin)
+BOOST_DLL_ALIAS(create_voxel_plugin, create_plugin)
 
 }  // namespace costmap_2d
diff --git a/src/costmap_2d.cpp b/src/costmap_2d.cpp
index 7494e57..e94a043 100644
--- a/src/costmap_2d.cpp
+++ b/src/costmap_2d.cpp
@@ -46,7 +46,9 @@ void Costmap2D::deleteMaps()
 void Costmap2D::initMaps(unsigned int size_x, unsigned int size_y)
 {
   boost::unique_lock<mutex_t> lock(*access_);
-  delete[] costmap_;                      // Xóa vùng nhớ cũ nếu có
+  if (costmap_ != nullptr) {
+    delete[] costmap_;
+  }
   costmap_ = new unsigned char[size_x * size_y];  // Mỗi ô lưu 1 byte (0–255)
 }
 
diff --git a/src/layer.cpp b/src/layer.cpp
index dd8d1ea..06e2c0d 100644
--- a/src/layer.cpp
+++ b/src/layer.cpp
@@ -3,6 +3,8 @@
 namespace costmap_2d
 {
 
+  
+
 Layer::Layer()
   : layered_costmap_(NULL)
   , current_(false)
diff --git a/src/layered_costmap.cpp b/src/layered_costmap.cpp
index 258d30a..a94d727 100644
--- a/src/layered_costmap.cpp
+++ b/src/layered_costmap.cpp
@@ -4,6 +4,7 @@
 #include <string>
 #include <algorithm>
 #include <vector>
+#include <memory>
 
 using std::vector;
 
@@ -48,7 +49,7 @@ namespace costmap_2d
     boost::unique_lock<Costmap2D::mutex_t> lock(*(costmap_.getMutex()));
     size_locked_ = size_locked;
     costmap_.resizeMap(size_x, size_y, resolution, origin_x, origin_y);
-    for (vector<boost::shared_ptr<Layer>>::iterator plugin = plugins_.begin(); plugin != plugins_.end();
+    for (vector<costmap_2d::PluginLayerPtr>::iterator plugin = plugins_.begin(); plugin != plugins_.end();
          ++plugin)
     {
       (*plugin)->matchSize();
@@ -75,7 +76,7 @@ namespace costmap_2d
     minx_ = miny_ = 1e30;
     maxx_ = maxy_ = -1e30;
 
-    for (vector<boost::shared_ptr<Layer>>::iterator plugin = plugins_.begin(); plugin != plugins_.end();
+    for (vector<costmap_2d::PluginLayerPtr>::iterator plugin = plugins_.begin(); plugin != plugins_.end();
          ++plugin)
     {
       if (!(*plugin)->isEnabled())
@@ -111,7 +112,7 @@ namespace costmap_2d
 
     costmap_.resetMap(x0, y0, xn, yn);
     
-    for (vector<boost::shared_ptr<Layer>>::iterator plugin = plugins_.begin(); plugin != plugins_.end();
+    for (vector<costmap_2d::PluginLayerPtr>::iterator plugin = plugins_.begin(); plugin != plugins_.end();
          ++plugin)
     {
       if ((*plugin)->isEnabled())
@@ -129,7 +130,7 @@ namespace costmap_2d
   bool LayeredCostmap::isCurrent()
   {
     current_ = true;
-    for (vector<boost::shared_ptr<Layer>>::iterator plugin = plugins_.begin(); plugin != plugins_.end();
+    for (vector<costmap_2d::PluginLayerPtr>::iterator plugin = plugins_.begin(); plugin != plugins_.end();
          ++plugin)
     {
       if ((*plugin)->isEnabled())
@@ -143,7 +144,7 @@ namespace costmap_2d
     footprint_ = footprint_spec;
     costmap_2d::calculateMinAndMaxDistances(footprint_spec, inscribed_radius_, circumscribed_radius_);
 
-    for (vector<boost::shared_ptr<Layer>>::iterator plugin = plugins_.begin(); plugin != plugins_.end();
+    for (vector<costmap_2d::PluginLayerPtr>::iterator plugin = plugins_.begin(); plugin != plugins_.end();
          ++plugin)
     {
       (*plugin)->onFootprintChanged();
diff --git a/src/observation_buffer.cpp b/src/observation_buffer.cpp
index 6432a3f..87b8d38 100644
--- a/src/observation_buffer.cpp
+++ b/src/observation_buffer.cpp
@@ -224,7 +224,6 @@ bool ObservationBuffer::isCurrent() const
         expected_update_rate_.toSec());
   }
   return current;
-  return true;
 }
 
 void ObservationBuffer::resetLastUpdated()
diff --git a/test/static_layer_test.cpp b/test/static_layer_test.cpp
index 8b44b3a..e3c9a8b 100644
--- a/test/static_layer_test.cpp
+++ b/test/static_layer_test.cpp
@@ -14,195 +14,459 @@ using namespace costmap_2d;
 
 int main(int argc, char* argv[]) {
 
-    // auto creator = boost::dll::import_alias<costmap_2d::PluginCostmapLayerPtr()>(
-    //     "./costmap_2d/libstatic_layer.so", "create_plugin", boost::dll::load_mode::append_decorations
-    // );
-
-    // costmap_2d::PluginCostmapLayerPtr plugin = creator();
-    // std::cout << "Plugin created successfully" << std::endl;
-    
-    // std::string global_frame = "map";
-    // bool rolling_window = true;
-    // bool track_unknown = true;
-    // LayeredCostmap layered_costmap(global_frame, rolling_window, track_unknown);
-    
-    // tf2::BufferCore tf_buffer;
-    // // tf2::Duration cache_time(10.0);
-    // // tf2::BufferCore tf_buffer(cache_time);
-    
-    // plugin->initialize(&layered_costmap, "static_layer", &tf_buffer);
-    
-    // std::cout << "Plugin initialized" << std::endl;
-    
-    // // plugin->activate();
-    // nav_msgs::OccupancyGrid grid;
-
-    // grid.info.resolution = 0.05f;
-    // grid.info.width = 2;
-    // grid.info.height = 2;
-
-    // grid.data.resize(grid.info.width * grid.info.height, -1);
-    // grid.data[0] = 0; 
-    // grid.data[1] = 100;
-    // grid.data[2] = 10;
-    // grid.data[3] = 0;
-    // plugin->dataCallBack(grid, "map");
-
-    // map_msgs::OccupancyGridUpdate update;
-
-    // update.x = 1;
-    // update.y = 1;
-    // update.width = 2;
-    // update.height = 2;
-
-    // update.data.resize(update.width * update.height, -1);
-    // update.data[0] = 0; 
-    // update.data[1] = 100;
-    // update.data[2] = 10;
-    // update.data[3] = 0;
-    // plugin->dataCallBack(update, "map_update");
-
-
-    // creator = boost::dll::import_alias<costmap_2d::PluginCostmapLayerPtr()>(
-    //     "./costmap_2d/libobstacle_layer.so", "create_plugin", boost::dll::load_mode::append_decorations
-    // );
-
-    // plugin = creator();
-    // std::cout << "Plugin created successfully" << std::endl;
-
-    // plugin->initialize(&layered_costmap, "obstacle_layer", &tf_buffer);
-
-    // sensor_msgs::LaserScan scan;
-
-    // // --- Header ---
-    // scan.header.seq = 1;
-    // scan.header.stamp = robot::Time::now();  // nếu bạn chưa có Time class, gán tạm số
-    // scan.header.frame_id = "laser_frame";
-
-    // // --- Cấu hình góc ---
-    // scan.angle_min = -M_PI;             // -180 độ
-    // scan.angle_max = M_PI;              // +180 độ
-    // scan.angle_increment = M_PI / 180;  // 1 độ mỗi tia
-
-    // // --- Cấu hình thời gian ---
-    // scan.time_increment = 0.0001f;  // mỗi tia cách nhau 0.1 ms
-    // scan.scan_time = 0.04f;         // tổng thời gian quét 40 ms (~25Hz)
-
-    // // --- Giới hạn đo ---
-    // scan.range_min = 0.1f;
-    // scan.range_max = 10.0f;
-
-    // // --- Tạo dữ liệu ---
-    // int num_readings = static_cast<int>((scan.angle_max - scan.angle_min) / scan.angle_increment);
-    // scan.ranges.resize(num_readings);
-    // scan.intensities.resize(num_readings);
-
-    // for (int i = 0; i < num_readings; ++i)
-    // {
-    //     float angle = scan.angle_min + i * scan.angle_increment;
-
-    //     // Tạo dữ liệu giả lập: vật thể cách 2m, dao động nhẹ theo góc
-    //     scan.ranges[i] = 2.0f + 0.2f * std::sin(3.0f * angle);
-
-    //     // Giả lập cường độ phản xạ
-    //     scan.intensities[i] = 100.0f + 20.0f * std::cos(angle);
-    // }
-
-    // // --- In thử 10 giá trị đầu ---
-    // // for (int i = 0; i < 10; ++i)
-    // // {
-    // //     std::cout << "Angle " << (scan.angle_min + i * scan.angle_increment)
-    // //               << " rad | Range " << scan.ranges[i]
-    // //               << " m | Intensity " << scan.intensities[i]
-    // //               << std::endl;
-    // // }
-    // plugin->deactivate();
-    // plugin->dataCallBack(scan, "laser_valid");
-
-    // sensor_msgs::PointCloud cloud;
-
-    // // 2. Thiết lập header
-    // cloud.header.stamp.sec = 1234567890;   // hoặc ros::Time::now().sec
-    // cloud.header.stamp.nsec = 0;
-    // cloud.header.frame_id = "laser_frame";
-
-    // // 3. Thêm một vài điểm
-    // geometry_msgs::Point32 pt1;
-    // pt1.x = 1.0f; pt1.y = 0.0f; pt1.z = 0.0f;
-
-    // geometry_msgs::Point32 pt2;
-    // pt2.x = 0.0f; pt2.y = 1.0f; pt2.z = 0.0f;
-
-    // geometry_msgs::Point32 pt3;
-    // pt3.x = 1.0f; pt3.y = 1.0f; pt3.z = 0.0f;
-
-    // cloud.points.push_back(pt1);
-    // cloud.points.push_back(pt2);
-    // cloud.points.push_back(pt3);
-
-    // // 4. Thêm dữ liệu channels (tùy chọn)
-    // cloud.channels.resize(1);
-    // cloud.channels[0].name = "intensity";
-    // cloud.channels[0].values.push_back(0.5f);
-    // cloud.channels[0].values.push_back(1.0f);
-    // cloud.channels[0].values.push_back(0.8f);
-
-    // plugin->dataCallBack(cloud, "pcl_cb");
-
-
-
-
-
-    
-    // // std::cout << "Plugin activated successfully" << std::endl;
-    // auto creator = boost::dll::import_alias<costmap_2d::PluginLayerPtr()>(
-    //     "./costmap_2d/libinflation_layer.so", "create_plugin_layer", boost::dll::load_mode::append_decorations
-    // );
-
-    // costmap_2d::PluginLayerPtr plugin = creator();
-    // std::cout << "Plugin created successfully" << std::endl;
-
-    // std::string global_frame = "map";
-    // bool rolling_window = true;
-    // bool track_unknown = true;
-    // LayeredCostmap layered_costmap(global_frame, rolling_window, track_unknown);
-    
-    // tf2::BufferCore tf_buffer;
-    // // tf2::Duration cache_time(10.0);
-    // // tf2::BufferCore tf_buffer(cache_time);
-    
-    // plugin->initialize(&layered_costmap, "inflation_layer", &tf_buffer);
-    // plugin->onFootprintChanged();
-    
-    // std::cout << "Plugin initialized" << std::endl;
-
-
-    auto creator_1 = boost::dll::import_alias<costmap_2d::PluginStaticLayerPtr()>(
-        "./costmap_2d/libpreferred_layer.so", "create_plugin_static_layer", boost::dll::load_mode::append_decorations
-    );
-
-    costmap_2d::PluginStaticLayerPtr plugin_1 = creator_1();
-    std::cout << "Plugin created successfully" << std::endl;
-
     std::string global_frame = "map";
-    bool rolling_window = true;
-    bool track_unknown = true;
+    bool rolling_window = false;
+    bool track_unknown = false;
     LayeredCostmap layered_costmap(global_frame, rolling_window, track_unknown);
     
-    tf2::BufferCore tf_buffer;
-    plugin_1->initialize(&layered_costmap, "critical_layer", &tf_buffer);
-    nav_msgs::OccupancyGrid grid;
-
-    grid.info.resolution = 0.05f;
-    grid.info.width = 2;
-    grid.info.height = 2;
-
-    grid.data.resize(grid.info.width * grid.info.height, -1);
-    grid.data[0] = 0; 
-    grid.data[1] = 100;
-    grid.data[2] = 10;
-    grid.data[3] = 0;
-    plugin_1->CostmapLayer::dataCallBack(grid, "map");
+    // ✅ FIX 1: RESIZE costmap TRƯỚC KHI sử dụng
+    // Costmap mặc định có size = 0x0, resolution = 0.0 -> sẽ gây crash
+    layered_costmap.resizeMap(
+        100,    // size_x: số ô theo chiều X
+        100,    // size_y: số ô theo chiều Y
+        0.05,   // resolution: 0.05m/ô (5cm)
+        0.0,    // origin_x
+        0.0,    // origin_y
+        false   // size_locked
+    );
+    std::cout << "Costmap resized to: " 
+              << layered_costmap.getCostmap()->getSizeInCellsX() 
+              << "x" << layered_costmap.getCostmap()->getSizeInCellsY()
+              << ", resolution: " << layered_costmap.getCostmap()->getResolution() 
+              << "m/cell" << std::endl;
     
-    std::cout << "Plugin initialized" << std::endl;
+    tf2::BufferCore tf_buffer;
+    
+    // // ✅ FIX 2: Lưu tất cả plugins để tránh bị destroy
+    // std::vector<costmap_2d::PluginLayerPtr> plugin_instances;
+    
+    // // // ✅ FIX 3: Load và add static_layer
+    // // auto creator = boost::dll::import_alias<costmap_2d::PluginLayerPtr()>(
+    // //     "./costmap_2d/libstatic_layer.so", "create_plugin", boost::dll::load_mode::append_decorations
+    // // );
+    // // costmap_2d::PluginLayerPtr plugin1 = creator();
+    // // std::cout << "Plugin created static_layer successfully" << std::endl;
+    // // plugin1->initialize(&layered_costmap, "static_layer", &tf_buffer);
+    // // layered_costmap.addPlugin(plugin1);  // ✅ FIX: ADD vào LayeredCostmap
+    // // plugin_instances.push_back(plugin1);  // ✅ FIX: Lưu lại để tránh bị destroy
+    // // std::cout << "Static layer initialized and added. Use count: " << plugin1.use_count() << std::endl;
+
+
+    // // // ✅ FIX 5: Load và add obstacle_layer
+    // // auto creator2 = boost::dll::import_alias<costmap_2d::PluginLayerPtr()>(
+    // //     "./costmap_2d/libobstacle_layer.so", "create_plugin", boost::dll::load_mode::append_decorations
+    // // );
+    // // costmap_2d::PluginLayerPtr plugin3 = creator2();
+    // // std::cout << "Plugin created obstacle_layer successfully" << std::endl;
+    // // plugin3->initialize(&layered_costmap, "obstacle_layer", &tf_buffer);
+    // // layered_costmap.addPlugin(plugin3);  // ✅ FIX: ADD vào LayeredCostmap
+    // // plugin_instances.push_back(plugin3);  // ✅ FIX: Lưu lại
+    // // std::cout << "Obstacle layer initialized and added. Use count: " << plugin3.use_count() << std::endl;
+
+
+    // // ✅ FIX 4: Load và add inflation_layer
+    // auto creator1 = boost::dll::import_alias<costmap_2d::PluginLayerPtr()>(
+    //     "./costmap_2d/libinflation_layer.so", "create_plugin", boost::dll::load_mode::append_decorations
+    // );
+    // costmap_2d::PluginLayerPtr plugin2 = creator1();
+    // std::cout << "✅ Plugin created inflation_layer successfully" << std::endl;
+    // plugin2->initialize(&layered_costmap, "inflation_layer", &tf_buffer);
+    // plugin2->onFootprintChanged();  // ✅ FIX: Cần gọi cho inflation_layer
+    // layered_costmap.addPlugin(plugin2);  // ✅ FIX: ADD vào LayeredCostmap
+    // plugin_instances.push_back(plugin2);  // ✅ FIX: Lưu lại
+    // std::cout << "Inflation layer initialized and added. Use count: " << plugin2.use_count() << std::endl;
+
+    // auto creator3 = boost::dll::import_alias<costmap_2d::PluginLayerPtr()>(
+    //     "./costmap_2d/libpreferred_layer.so", "create_plugin", boost::dll::load_mode::append_decorations
+    // );
+    // costmap_2d::PluginLayerPtr plugin4 = creator3();
+    // std::cout << "Plugin created preferred_layer successfully" << std::endl;
+    // plugin4->initialize(&layered_costmap, "preferred_layer", &tf_buffer);
+    // layered_costmap.addPlugin(plugin4);  // ✅ FIX: ADD vào LayeredCostmap
+    // plugin_instances.push_back(plugin4);  // ✅ FIX: Lưu lại
+    
+
+    // // ✅ Kiểm tra số lượng plugins
+    // std::cout << "Total plugins added: " << layered_costmap.getPlugins()->size() << std::endl;
+    
+    // // ✅ Test updateMap (nếu cần)
+    // // layered_costmap.updateMap(0.0, 0.0, 0.0);  // robot_x, robot_y, robot_yaw
+    
+    // std::cout << "All plugins initialized and added successfully!" << std::endl;
+    
+    // // ✅ FIX: Xóa reference đến layered_costmap trong các plugins trước khi destroy
+    // // Để tránh destructor của plugin cố gắng truy cập layered_costmap đã bị destroy
+    // // for (auto& plugin : plugin_instances) {
+    // //     // Reset pointer để tránh truy cập vào object đã bị destroy
+    // //     // Lưu ý: Cần kiểm tra xem Layer có hàm reset pointer không
+    // //     // Nếu không có, cần thêm vào Layer class
+    // // }
+    
+    // // ✅ FIX: Clear plugin_instances TRƯỚC KHI layered_costmap bị destroy
+    // // Thứ tự destroy: plugin_instances -> plugins (trong shared_ptr) -> destructor của plugin
+    // plugin_instances.clear();
+    // std::cout << "Plugin instances cleared" << std::endl;
+    // // std::cout << "=== Costmap Plugin Test ===" << std::endl;
+
+    // // // 1️⃣ Khởi tạo LayeredCostmap
+    // // std::string global_frame = "map";
+    // // bool rolling_window = true;
+    // // bool track_unknown = true;
+    // // LayeredCostmap layered_costmap(global_frame, rolling_window, track_unknown);
+
+    // // // ✅ Resize map trước khi init plugin
+    // // layered_costmap.resizeMap(
+    // //     100,    // size_x
+    // //     100,    // size_y
+    // //     0.05,   // resolution (m/cell)
+    // //     0.0,    // origin_x
+    // //     0.0,    // origin_y
+    // //     false   // size_locked
+    // // );
+
+    // // std::cout << "Costmap resized to: "
+    // //           << layered_costmap.getCostmap()->getSizeInCellsX()
+    // //           << "x" << layered_costmap.getCostmap()->getSizeInCellsY()
+    // //           << ", resolution: " << layered_costmap.getCostmap()->getResolution()
+    // //           << " m/cell" << std::endl;
+
+    // // // 2️⃣ TF buffer
+    // // tf2::BufferCore tf_buffer;
+
+    // // // 3️⃣ Thiết lập footprint robot trước khi load inflation_layer
+    // // std::vector<geometry_msgs::Point> footprint = {
+    // //     {0.1, 0.1, 0.0},
+    // //     {0.1, -0.1, 0.0},
+    // //     {-0.1, -0.1, 0.0},
+    // //     {-0.1, 0.1, 0.0}
+    // // };
+    // // // layered_costmap.setUnpaddedRobotFootprint(footprint);
+
+    // 4️⃣ Danh sách plugin để load (thứ tự an toàn)
+    struct PluginInfo { std::string path; std::string name; };
+    std::vector<PluginInfo> plugins_to_load = {
+        {"./costmap_2d/libstatic_layer.so", "static_layer"},
+        {"./costmap_2d/libinflation_layer.so", "inflation_layer"},
+        {"./costmap_2d/libobstacle_layer.so", "obstacle_layer"},
+        {"./costmap_2d/libpreferred_layer.so", "preferred_layer"}
+    };
+
+    // // // 5️⃣ Vector lưu shared_ptr plugin
+    std::vector<PluginLayerPtr> plugin_instances;
+
+    for (auto& info : plugins_to_load)
+    {
+        auto creator = boost::dll::import_alias<PluginLayerPtr()>(
+            info.path, "create_plugin", boost::dll::load_mode::append_decorations
+        );
+
+        PluginLayerPtr plugin = creator();
+        std::cout << "Plugin created: " << info.name << std::endl;
+
+        plugin->initialize(&layered_costmap, info.name, &tf_buffer);
+
+        // Nếu là inflation_layer, cần gọi onFootprintChanged()
+        if(info.name == "inflation_layer")
+        {
+            plugin->onFootprintChanged();
+            std::cout << "Inflation layer footprint updated." << std::endl;
+        }
+
+        plugin_instances.push_back(plugin);
+        layered_costmap.addPlugin(plugin); 
+        std::cout << info.name << " initialized, use_count=" << plugin.use_count() << std::endl;
+    }
+
+    std::cout << "All plugins loaded successfully." << std::endl;
+
+    // 6️⃣ Test update map với dummy grid
+    // nav_msgs::OccupancyGrid grid;
+    // grid.info.width = 10;
+    // grid.info.height = 10;
+    // grid.info.resolution = 0.05;
+    // grid.data.resize(grid.info.width * grid.info.height, 0); // free space
+
+    // for(auto& plugin : plugin_instances)
+    // {
+    //     plugin->dataCallBack(grid, "map");
+    // }
+
+    // std::cout << "Map update simulated." << std::endl;
+
+    // // // 7️⃣ Clear plugin_instances trước khi LayeredCostmap bị destroy
+    // // plugin_instances.clear();
+    // // std::cout << "Plugin instances cleared safely." << std::endl;
+
+    // // return 0;
+
+    // auto creator1 = boost::dll::import_alias<costmap_2d::PluginLayerPtr()>(
+    //     "./costmap_2d/libobstacle_layer.so", "create_plugin", boost::dll::load_mode::append_decorations
+    // );
+    // costmap_2d::PluginLayerPtr plugin1 = creator1();
+    // std::cout << "Plugin created obstacle_layer successfully" << std::endl;
+    // plugin1->initialize(&layered_costmap, "obstacle_layer", &tf_buffer);
+    // layered_costmap.addPlugin(plugin1); 
+
+
+    // auto creator2 = boost::dll::import_alias<costmap_2d::PluginLayerPtr()>(
+    //     "./costmap_2d/libstatic_layer.so", "create_plugin", boost::dll::load_mode::append_decorations
+    // );
+    // costmap_2d::PluginLayerPtr plugin2 = creator2();
+    // std::cout << "Plugin created static_layer successfully" << std::endl;
+    // plugin2->initialize(&layered_costmap, "static_layer", &tf_buffer);
+    // layered_costmap.addPlugin(plugin2); 
+
+
+    // auto creator3 = boost::dll::import_alias<costmap_2d::PluginLayerPtr()>(
+    //     "./costmap_2d/libinflation_layer.so", "create_plugin", boost::dll::load_mode::append_decorations
+    // );
+    // costmap_2d::PluginLayerPtr plugin3 = creator3();
+    // std::cout << "Plugin created inflation_layer successfully" << std::endl;
+    // plugin3->initialize(&layered_costmap, "inflation_layer", &tf_buffer);
+    // layered_costmap.addPlugin(plugin3); 
+
+    // auto creator4 = boost::dll::import_alias<costmap_2d::PluginLayerPtr()>(
+    //     "./costmap_2d/libpreferred_layer.so", "create_plugin", boost::dll::load_mode::append_decorations
+    // );
+    // costmap_2d::PluginLayerPtr plugin4 = creator4();
+    // std::cout << "Plugin created preferred_layer successfully" << std::endl;
+    // plugin4->initialize(&layered_costmap, "preferred_layer", &tf_buffer);
+    // layered_costmap.addPlugin(plugin4); 
+
+    // while(true)
+    // {
+    robot::Duration(10).sleep();
+    // }
+
+    return 0;
 }
+
+
+
+
+
+
+
+    // auto creator = boost::dll::import_alias<costmap_2d::PluginLayerPtr()>(
+    //     "./costmap_2d/libobstacle_layer.so", "create_plugin", boost::dll::load_mode::append_decorations
+    // );
+    // costmap_2d::PluginLayerPtr plugin = creator();
+    // std::cout << "Plugin created obstacle_layer successfully" << std::endl;
+    // plugin->initialize(&layered_costmap, "obstacle_layer", &tf_buffer);
+
+
+    // creator = boost::dll::import_alias<costmap_2d::PluginLayerPtr()>(
+    //     "./costmap_2d/libstatic_layer.so", "create_plugin", boost::dll::load_mode::append_decorations
+    // );
+    // plugin = creator();
+    // std::cout << "Plugin created static_layer successfully" << std::endl;
+    // plugin->initialize(&layered_costmap, "static_layer", &tf_buffer);
+
+
+    // creator = boost::dll::import_alias<costmap_2d::PluginLayerPtr()>(
+    //     "./costmap_2d/libinflation_layer.so", "create_plugin", boost::dll::load_mode::append_decorations
+    // );
+    // plugin = creator();
+    // std::cout << "Plugin created inflation_layer successfully" << std::endl;
+    // plugin->initialize(&layered_costmap, "inflation_layer", &tf_buffer);
+
+
+    // creator = boost::dll::import_alias<costmap_2d::PluginLayerPtr()>(
+    //     "./costmap_2d/libpreferred_layer.so", "create_plugin", boost::dll::load_mode::append_decorations
+    // );
+    // plugin = creator();
+    // std::cout << "Plugin created preferred_layer successfully" << std::endl;
+    // plugin->initialize(&layered_costmap, "preferred_layer", &tf_buffer);
+
+
+
+
+
+
+// #include <boost/dll/import.hpp>
+// #include <iostream>
+// #include <costmap_2d/costmap_layer.h>
+// #include <costmap_2d/layered_costmap.h>
+// #include <tf2/buffer_core.h>
+// #include <tf2/time.h>
+// #include "nav_msgs/OccupancyGrid.h"
+// #include <map_msgs/OccupancyGridUpdate.h>
+// #include <costmap_2d/obstacle_layer.h>
+// #include <costmap_2d/inflation_layer.h>
+// #include <costmap_2d/critical_layer.h>
+// #include <costmap_2d/preferred_layer.h>
+// using namespace costmap_2d;
+
+// int main(int argc, char* argv[]) {
+
+//     auto creator = boost::dll::import_alias<costmap_2d::PluginLayerPtr()>(
+//         "./costmap_2d/libstatic_layer.so", "create_plugin", boost::dll::load_mode::append_decorations
+//     );
+
+//     costmap_2d::PluginLayerPtr plugin = creator();
+//     std::cout << "Plugin created static_layer successfully" << std::endl;
+    
+//     std::string global_frame = "map";
+//     bool rolling_window = true;
+//     bool track_unknown = true;
+//     LayeredCostmap layered_costmap(global_frame, rolling_window, track_unknown);
+    
+//     tf2::BufferCore tf_buffer;
+//     // tf2::Duration cache_time(10.0);
+//     // tf2::BufferCore tf_buffer(cache_time);
+    
+//     plugin->initialize(&layered_costmap, "static_layer", &tf_buffer);
+    
+//     std::cout << "Plugin initialized" << std::endl;
+    
+//     // plugin->activate();
+//     nav_msgs::OccupancyGrid grid;
+
+//     grid.info.resolution = 0.05f;
+//     grid.info.width = 2;
+//     grid.info.height = 2;
+
+//     grid.data.resize(grid.info.width * grid.info.height, -1);
+//     grid.data[0] = 0; 
+//     grid.data[1] = 100;
+//     grid.data[2] = 10;
+//     grid.data[3] = 0;
+//     plugin->dataCallBack(grid, "map");
+
+//     map_msgs::OccupancyGridUpdate update;
+
+//     update.x = 1;
+//     update.y = 1;
+//     update.width = 2;
+//     update.height = 2;
+
+//     update.data.resize(update.width * update.height, -1);
+//     update.data[0] = 0; 
+//     update.data[1] = 100;
+//     update.data[2] = 10;
+//     update.data[3] = 0;
+//     plugin->dataCallBack(update, "map_update");
+
+
+//     // creator = boost::dll::import_alias<costmap_2d::PluginLayerPtr()>(
+//     //     "./costmap_2d/libobstacle_layer.so", "create_plugin", boost::dll::load_mode::append_decorations
+//     // );
+
+//     // plugin = creator();
+//     // std::cout << "Plugin created obstacle_layer successfully" << std::endl;
+
+//     // plugin->initialize(&layered_costmap, "obstacle_layer", &tf_buffer);
+
+//     // sensor_msgs::LaserScan scan;
+
+//     // // --- Header ---
+//     // scan.header.seq = 1;
+//     // scan.header.stamp = robot::Time::now();  // nếu bạn chưa có Time class, gán tạm số
+//     // scan.header.frame_id = "laser_frame";
+
+//     // // --- Cấu hình góc ---
+//     // scan.angle_min = -M_PI;             // -180 độ
+//     // scan.angle_max = M_PI;              // +180 độ
+//     // scan.angle_increment = M_PI / 180;  // 1 độ mỗi tia
+
+//     // // --- Cấu hình thời gian ---
+//     // scan.time_increment = 0.0001f;  // mỗi tia cách nhau 0.1 ms
+//     // scan.scan_time = 0.04f;         // tổng thời gian quét 40 ms (~25Hz)
+
+//     // // --- Giới hạn đo ---
+//     // scan.range_min = 0.1f;
+//     // scan.range_max = 10.0f;
+
+//     // // --- Tạo dữ liệu ---
+//     // int num_readings = static_cast<int>((scan.angle_max - scan.angle_min) / scan.angle_increment);
+//     // scan.ranges.resize(num_readings);
+//     // scan.intensities.resize(num_readings);
+
+//     // for (int i = 0; i < num_readings; ++i)
+//     // {
+//     //     float angle = scan.angle_min + i * scan.angle_increment;
+
+//     //     // Tạo dữ liệu giả lập: vật thể cách 2m, dao động nhẹ theo góc
+//     //     scan.ranges[i] = 2.0f + 0.2f * std::sin(3.0f * angle);
+
+//     //     // Giả lập cường độ phản xạ
+//     //     scan.intensities[i] = 100.0f + 20.0f * std::cos(angle);
+//     // }
+
+//     // // --- In thử 10 giá trị đầu ---
+//     // // for (int i = 0; i < 10; ++i)
+//     // // {
+//     // //     std::cout << "Angle " << (scan.angle_min + i * scan.angle_increment)
+//     // //               << " rad | Range " << scan.ranges[i]
+//     // //               << " m | Intensity " << scan.intensities[i]
+//     // //               << std::endl;
+//     // // }
+//     // // plugin->deactivate();
+//     // plugin->dataCallBack(scan, "laser_valid");
+
+//     // sensor_msgs::PointCloud cloud;
+
+//     // // 2. Thiết lập header
+//     // cloud.header.stamp.sec = 1234567890;   // hoặc ros::Time::now().sec
+//     // cloud.header.stamp.nsec = 0;
+//     // cloud.header.frame_id = "laser_frame";
+
+//     // // 3. Thêm một vài điểm
+//     // geometry_msgs::Point32 pt1;
+//     // pt1.x = 1.0f; pt1.y = 0.0f; pt1.z = 0.0f;
+
+//     // geometry_msgs::Point32 pt2;
+//     // pt2.x = 0.0f; pt2.y = 1.0f; pt2.z = 0.0f;
+
+//     // geometry_msgs::Point32 pt3;
+//     // pt3.x = 1.0f; pt3.y = 1.0f; pt3.z = 0.0f;
+
+//     // cloud.points.push_back(pt1);
+//     // cloud.points.push_back(pt2);
+//     // cloud.points.push_back(pt3);
+
+//     // // 4. Thêm dữ liệu channels (tùy chọn)
+//     // cloud.channels.resize(1);
+//     // cloud.channels[0].name = "intensity";
+//     // cloud.channels[0].values.push_back(0.5f);
+//     // cloud.channels[0].values.push_back(1.0f);
+//     // cloud.channels[0].values.push_back(0.8f);
+
+//     // plugin->dataCallBack(cloud, "pcl_cb");
+    
+//     std::cout << "Plugin activated successfully" << std::endl;
+//     creator = boost::dll::import_alias<costmap_2d::PluginLayerPtr()>(
+//         "./costmap_2d/libinflation_layer.so", "create_plugin", boost::dll::load_mode::append_decorations
+//     );
+
+//     plugin = creator();
+//     std::cout << "Plugin created inflation_layer successfully" << std::endl;
+    
+//     plugin->initialize(&layered_costmap, "inflation_layer", &tf_buffer);
+//     plugin->onFootprintChanged();
+    
+//     std::cout << "Plugin initialized" << std::endl;
+
+
+//     creator = boost::dll::import_alias<costmap_2d::PluginLayerPtr()>(
+//         "./costmap_2d/libpreferred_layer.so", "create_plugin", boost::dll::load_mode::append_decorations
+//     );
+
+//     plugin = creator();
+//     std::cout << "Plugin created preferred_layer successfully" << std::endl;
+
+//     plugin->initialize(&layered_costmap, "preferred_layer", &tf_buffer);
+//     plugin->dataCallBack(grid, "map");
+
+    
+//     creator = boost::dll::import_alias<costmap_2d::PluginLayerPtr()>(
+//         "./costmap_2d/libobstacle_layer.so", "create_plugin", boost::dll::load_mode::append_decorations
+//     );
+
+//     plugin = creator();
+//     std::cout << "Plugin created obstacle_layer successfully" << std::endl;
+
+//     plugin->initialize(&layered_costmap, "obstacle_layer", &tf_buffer);
+    
+//     std::cout << "Plugin initialized" << std::endl;
+// }
\ No newline at end of file