costmap_2d/CODE_REVIEW.md

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

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:

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:

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:

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:

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

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:

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

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:

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

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:

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

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

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:

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

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:

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

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

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:

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:

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

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()