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
Download Patch File Download Diff File Expand all files Collapse all files

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