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

update file obstacle_layer and file test plugin

Browse Source
This commit is contained in:
duongtd
2025-11-12 17:38:38 +07:00
parent 19683269c3
commit c94de60a7b
12 changed files with 393 additions and 517 deletions
Download Patch File Download Diff File Expand all files Collapse all files

96
test/static_layer_test.cpp
View File

@@ -6,12 +6,13 @@
#include <tf2/time.h>
#include "nav_msgs/OccupancyGrid.h"
#include <map_msgs/OccupancyGridUpdate.h>
#include <costmap_2d/obstacle_layer.h>
using namespace costmap_2d;
int main(int argc, char* argv[]) {
auto creator = boost::dll::import_alias<costmap_2d::PluginCostmapLayerPtr()>(
"./costmap_2d/liblayers.so", "create_plugin", boost::dll::load_mode::append_decorations
"./costmap_2d/libstatic_layer.so", "create_plugin", boost::dll::load_mode::append_decorations
);
costmap_2d::PluginCostmapLayerPtr plugin = creator();
@@ -42,7 +43,7 @@ int main(int argc, char* argv[]) {
grid.data[1] = 100;
grid.data[2] = 10;
grid.data[3] = 0;
plugin->dataCallBack(grid);
plugin->dataCallBack(grid, "map");
map_msgs::OccupancyGridUpdate update;
@@ -56,7 +57,96 @@ int main(int argc, char* argv[]) {
update.data[1] = 100;
update.data[2] = 10;
update.data[3] = 0;
plugin->dataCallBack(update);
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");