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

update function dataCallBack and file test static layer

Browse Source
This commit is contained in:
duongtd
2025-11-11 14:28:20 +07:00
parent 498b606e15
commit 19683269c3
7 changed files with 213 additions and 48 deletions
Download Patch File Download Diff File Expand all files Collapse all files

38
test/static_layer_test.cpp
View File

@@ -5,11 +5,11 @@
#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[]) {
// Fix 1: Use correct path to the library
auto creator = boost::dll::import_alias<costmap_2d::PluginCostmapLayerPtr()>(
"./costmap_2d/liblayers.so", "create_plugin", boost::dll::load_mode::append_decorations
);
@@ -17,24 +17,19 @@ int main(int argc, char* argv[]) {
costmap_2d::PluginCostmapLayerPtr plugin = creator();
std::cout << "Plugin created successfully" << std::endl;
// Fix 2: Initialize the plugin before calling activate()
// activate() calls onInitialize() which requires layered_costmap_ to be set
std::string global_frame = "map";
bool rolling_window = false;
bool rolling_window = true;
bool track_unknown = true;
LayeredCostmap layered_costmap(global_frame, rolling_window, track_unknown);
// Fix 3: tf2::BufferCore constructor requires ros::Duration parameter (not tf2::Duration)
tf2::BufferCore tf_buffer;
// tf2::Duration cache_time(10.0); // 10 seconds cache time
// tf2::Duration cache_time(10.0);
// tf2::BufferCore tf_buffer(cache_time);
// Initialize the layer with required parameters
plugin->initialize(&layered_costmap, "static_layer", &tf_buffer);
std::cout << "Plugin initialized" << std::endl;
// Now it's safe to call activate()
// plugin->activate();
nav_msgs::OccupancyGrid grid;
@@ -43,11 +38,28 @@ int main(int argc, char* argv[]) {
grid.info.height = 2;
grid.data.resize(grid.info.width * grid.info.height, -1);
grid.data[0] = 0; // Free cell
grid.data[1] = 100; // Occupied cell
grid.data[2] = 10; // Occupied cell
grid.data[3] = 0; // Free cell
plugin->incomingMap(grid);
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;
}