209 lines
6.9 KiB
C++
209 lines
6.9 KiB
C++
#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::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;
|
|
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");
|
|
|
|
std::cout << "Plugin initialized" << std::endl;
|
|
}
|