441 lines
13 KiB
Markdown
441 lines
13 KiB
Markdown
# 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()`
|
|
|