DuongTD/costmap_2d
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

update file voxel_layer

Browse Source
This commit is contained in:
duongtd 2025-11-15 17:36:52 +07:00
parent bd98bf4e08
commit 49a72383c8
23 changed files with 977 additions and 253 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
CMakeLists.txt
View File

@ -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})

440
CODE_REVIEW.md Normal file
View File

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

28
include/costmap_2d/costmap_2d_robot.h
View File

@ -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

17
include/costmap_2d/costmap_layer.h
View File

@ -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_

10
include/costmap_2d/inflation_layer.h
View File

@ -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

19
include/costmap_2d/layer.h
View File

@ -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_

12
include/costmap_2d/layered_costmap.h
View File

@ -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

2
include/costmap_2d/obstacle_layer.h
View File

@ -149,8 +149,6 @@ private:
bool getParams();
};
using PluginObstacleLayerPtr = std::shared_ptr<ObstacleLayer>;
} // namespace costmap_2d
#endif // COSTMAP_2D_OBSTACLE_LAYER_H_

2
include/costmap_2d/static_layer.h
View File

@ -64,8 +64,6 @@ private:
bool getParams();
};
using PluginStaticLayerPtr = std::shared_ptr<StaticLayer>;
} // namespace costmap_2d
#endif // COSTMAP_2D_STATIC_LAYER_H_

2
include/costmap_2d/voxel_layer.h
View File

@ -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
{

4
plugins/critical_layer.cpp
View File

@ -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)
}

4
plugins/directional_layer.cpp
View File

@ -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)
}

11
plugins/inflation_layer.cpp
View File

@ -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

5
plugins/obstacle_layer.cpp
View File

@ -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>();
}

4
plugins/preferred_layer.cpp
View File

@ -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)
}

2
plugins/static_layer.cpp
View File

@ -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>();
}

4
plugins/unpreferred_layer.cpp
View File

@ -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)
}

4
plugins/voxel_layer.cpp
View File

@ -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

4
src/costmap_2d.cpp
View File

@ -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)
}

2
src/layer.cpp
View File

@ -3,6 +3,8 @@
namespace costmap_2d
{
Layer::Layer()
: layered_costmap_(NULL)
, current_(false)

11
src/layered_costmap.cpp
View File

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

1
src/observation_buffer.cpp
View File

@ -224,7 +224,6 @@ bool ObservationBuffer::isCurrent() const
expected_update_rate_.toSec());
}
return current;
return true;
}
void ObservationBuffer::resetLastUpdated()

638
test/static_layer_test.cpp
View File

@ -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;
// }