66 lines
1.8 KiB
C++
66 lines
1.8 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>
|
|
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::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_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);
|
|
|
|
|
|
|
|
|
|
std::cout << "Plugin activated successfully" << std::endl;
|
|
}
|