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

14
include/costmap_2d/costmap_layer.h
View File

@ -39,6 +39,7 @@
#define COSTMAP_2D_COSTMAP_LAYER_H_
#include <costmap_2d/layer.h>
#include <costmap_2d/layered_costmap.h>
#include <typeinfo>
namespace costmap_2d
{
@ -70,7 +71,20 @@ public:
*/
void addExtraBounds(double mx0, double my0, double mx1, double my1);
template<typename T>
void dataCallBack(const T& value) {
handle(value);
}
protected:
// Hàm template public, dùng để gửi dữ liệu
template<typename T>
void handle(const T& data) {
handleImpl(static_cast<const void*>(&data), typeid(T));
}
// Hàm ảo duy nhất mà plugin sẽ override
virtual void handleImpl(const void* data, const std::type_info& type) = 0;
/*
* Updates the master_grid within the specified
* bounding box using this layer's values.

4
include/costmap_2d/static_layer.h
View File

@ -30,6 +30,10 @@ public:
virtual void incomingMap(const nav_msgs::OccupancyGrid& new_map);
virtual void incomingUpdate(const map_msgs::OccupancyGridUpdate& update);
protected:
void handleImpl(const void* data, const std::type_info& type) override;
void process(const nav_msgs::OccupancyGrid& new_map);
void process(const map_msgs::OccupancyGridUpdate& update);
virtual unsigned char interpretValue(unsigned char value);
unsigned char* threshold_;
std::string base_frame_id_;

2
plugins/inflation_layer.cpp
View File

@ -362,7 +362,7 @@ void InflationLayer::setInflationParameters(double inflation_radius, double cost
}
// Export factory function
static PluginPtr create_inflation_plugin() {
static PluginCostmapLayerPtr create_inflation_plugin() {
return std::make_shared<InflationLayer>();
}

35
plugins/obstacle_layer.cpp
View File

@ -1,5 +1,6 @@
#include <costmap_2d/obstacle_layer.h>
#include <costmap_2d/costmap_math.h>
#include <costmap_2d/utils.h>
// #include <tf2_ros/message_filter.h>
#include <sensor_msgs/point_cloud2_iterator.h>
#include <tf2/utils.h>
@ -17,9 +18,11 @@ namespace costmap_2d
void ObstacleLayer::onInitialize()
{
getParams();
// ros::NodeHandle nh("~/" + name_), g_nh;
rolling_window_ = layered_costmap_->isRolling();
bool track_unknown_space;
// nh.param("track_unknown_space", track_unknown_space, layered_costmap_->isTrackingUnknown());
if (track_unknown_space)
@ -115,7 +118,7 @@ void ObstacleLayer::onInitialize()
> sub(new message_filters::Subscriber<sensor_msgs::LaserScan>(g_nh, topic, 50));
boost::shared_ptr<tf2_ros::MessageFilter<sensor_msgs::LaserScan> > filter(
new tf2_ros::MessageFilter<sensor_msgs::LaserScan>(*sub, *tf_, global_frame_, 50, g_nh));
new tf2_ros::MessageFilter<sensor_msgs::LaserScan>(*sub, *tf_, global_frame_,50 , g_nh));
if (inf_is_valid)
{
@ -190,13 +193,29 @@ void ObstacleLayer::onInitialize()
ObstacleLayer::~ObstacleLayer()
{}
// void ObstacleLayer::reconfigureCB(costmap_2d::ObstaclePluginConfig &config, uint32_t level)
// {
// enabled_ = config.enabled;
// footprint_clearing_enabled_ = config.footprint_clearing_enabled;
// max_obstacle_height_ = config.max_obstacle_height;
// combination_method_ = config.combination_method;
// }
bool StaticLayer::getParams()
{
try {
YAML::Node config = YAML::LoadFile("/home/duongtd/robotics_core/costmap_2d/config/config.yaml");
YAML::Node layer = config["obstacle_layer"];
bool enabled = loadParam(layer, "enabled", true);
bool footprint_clearing_enabled = loadParam(layer, "footprint_clearing_enabled", true);
double max_obstacle_height = loadParam(layer, "max_obstacle_height", 2);
int combination_method = loadParam(layer, "combination_method", 1);
enabled_ = enabled;
footprint_clearing_enabled_ = footprint_clearing_enabled;
max_obstacle_height_ = max_obstacle_height;
combination_method_ = combination_method;
}
catch (const YAML::BadFile& e) {
std::cerr << "Cannot open YAML file: " << e.what() << std::endl;
return false;
}
return true;
}
// void ObstacleLayer::laserScanCallback(const sensor_msgs::LaserScanConstPtr& message,
// const boost::shared_ptr<ObservationBuffer>& buffer)

116
plugins/static_layer.cpp
View File

@ -160,6 +160,122 @@ unsigned char StaticLayer::interpretValue(unsigned char value)
return scale * LETHAL_OBSTACLE;
}
void StaticLayer::handleImpl(const void* data, const std::type_info& type)
{
if (type == typeid(nav_msgs::OccupancyGrid)) {
process(*static_cast<const nav_msgs::OccupancyGrid*>(data));
} else if (type == typeid(map_msgs::OccupancyGridUpdate)) {
process(*static_cast<const map_msgs::OccupancyGridUpdate*>(data));
} else {
std::cout << "[Plugin] Unknown type: " << type.name() << std::endl;
}
}
void StaticLayer::process(const nav_msgs::OccupancyGrid& new_map)
{
if(!map_shutdown_)
{
std::cout << "Received new map!" << std::endl;
unsigned int size_x = new_map.info.width, size_y = new_map.info.height;
printf("Received a %d X %d map at %f m/pix\n", size_x, size_y, new_map.info.resolution);
// resize costmap if size, resolution or origin do not match
Costmap2D* master = layered_costmap_->getCostmap();
if (!layered_costmap_->isRolling() &&
(master->getSizeInCellsX() != size_x ||
master->getSizeInCellsY() != size_y ||
master->getResolution() != new_map.info.resolution ||
master->getOriginX() != new_map.info.origin.position.x ||
master->getOriginY() != new_map.info.origin.position.y))
{
// Update the size of the layered costmap (and all layers, including this one)
printf("Resizing costmap to %d X %d at %f m/pix\n", size_x, size_y, new_map.info.resolution);
layered_costmap_->resizeMap(size_x, size_y, new_map.info.resolution, new_map.info.origin.position.x,
new_map.info.origin.position.y,
true /* set size_locked to true, prevents reconfigureCb from overriding map size*/);
}
else if (size_x_ != size_x || size_y_ != size_y ||
resolution_ != new_map.info.resolution ||
origin_x_ != new_map.info.origin.position.x ||
origin_y_ != new_map.info.origin.position.y)
{
// only update the size of the costmap stored locally in this layer
printf("Resizing static layer to %d X %d at %f m/pix\n", size_x, size_y, new_map.info.resolution);
resizeMap(size_x, size_y, new_map.info.resolution,
new_map.info.origin.position.x, new_map.info.origin.position.y);
}
unsigned int index = 0;
// initialize the costmap with static data
for (unsigned int i = 0; i < size_y; ++i)
{
for (unsigned int j = 0; j < size_x; ++j)
{
unsigned char value = new_map.data[index];
costmap_[index] = interpretValue(value);
++index;
}
}
map_frame_ = new_map.header.frame_id;
// // Print current costmap
// std::cout << "[StaticLayer] Costmap (" << size_y_ << " x " << size_x_ << "):" << std::endl;
// for (unsigned int i = 0; i < size_y_; ++i)
// {
// for (unsigned int j = 0; j < size_x_; ++j)
// {
// unsigned int idx = i * size_x_ + j;
// std::cout << static_cast<int>(costmap_[idx]);
// if (j + 1 < size_x_) std::cout << ' ';
// }
// std::cout << std::endl;
// }
// we have a new map, update full size of map
x_ = y_ = 0;
width_ = size_x_;
height_ = size_y_;
map_received_ = true;
has_updated_data_ = true;
}
else
{
printf("Stop receive new map!");
}
// shutdown the map subscrber if firt_map_only_ flag is on
if (first_map_only_)
{
printf("Shutting down the map subscriber. first_map_only flag is on");
map_shutdown_ = true;
// map_sub_.shutdown();
}
}
void StaticLayer::process(const map_msgs::OccupancyGridUpdate& update)
{
if(!map_update_shutdown_)
{
std::cout << "Update new map!" << std::endl;
unsigned int di = 0;
for (unsigned int y = 0; y < update.height ; y++)
{
unsigned int index_base = (update.y + y) * size_x_;
for (unsigned int x = 0; x < update.width ; x++)
{
unsigned int index = index_base + x + update.x;
costmap_[index] = interpretValue(update.data[di++]);
}
}
x_ = update.x;
y_ = update.y;
width_ = update.width;
height_ = update.height;
has_updated_data_ = true;
}
}
void StaticLayer::incomingMap(const nav_msgs::OccupancyGrid& new_map)
{
if(!map_shutdown_)

52
src/observation_buffer.cpp
View File

@ -41,12 +41,12 @@ bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame)
std::string tf_error;
// if (!tf2_buffer_.canTransform(new_global_frame, global_frame_, tf2::Time::now(), &tf_error))
// {
// printf("Transform between %s and %s with tolerance %.2f failed: %s.", new_global_frame.c_str(),
// global_frame_.c_str(), tf_tolerance_, tf_error.c_str());
// return false;
// }
if (!tf2_buffer_.canTransform(new_global_frame, global_frame_, tf2::Time::now(), &tf_error))
{
printf("Transform between %s and %s with tolerance %.2f failed: %s.", new_global_frame.c_str(),
global_frame_.c_str(), tf_tolerance_, tf_error.c_str());
return false;
}
list<Observation>::iterator obs_it;
for (obs_it = observation_list_.begin(); obs_it != observation_list_.end(); ++obs_it)
@ -61,17 +61,17 @@ bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame)
origin.point = obs.origin_;
// we need to transform the origin of the observation to the new global frame
// tf2::doTransform(origin, origin,
// tf2_buffer_.lookupTransform(new_global_frame,
// tf2::getFrameId(origin),
// tf2::getTimestamp(origin)));
// obs.origin_ = origin.point;
tf2::doTransform(origin, origin,
tf2_buffer_.lookupTransform(new_global_frame,
tf2::getFrameId(origin),
tf2::getTimestamp(origin)));
obs.origin_ = origin.point;
// // we also need to transform the cloud of the observation to the new global frame
// tf2::doTransform(*(obs.cloud_), *(obs.cloud_),
// tf2_buffer_.lookupTransform(new_global_frame,
// tf2::getFrameId(*(obs.cloud_)),
// tf2::getTimestamp(*(obs.cloud_))));
// we also need to transform the cloud of the observation to the new global frame
tf2::doTransform(*(obs.cloud_), *(obs.cloud_),
tf2_buffer_.lookupTransform(new_global_frame,
tf2::getFrameId(*(obs.cloud_)),
tf2::getTimestamp(*(obs.cloud_))));
}
catch (TransformException& ex)
{
@ -105,10 +105,10 @@ void ObservationBuffer::bufferCloud(const sensor_msgs::PointCloud2& cloud)
local_origin.point.x = 0;
local_origin.point.y = 0;
local_origin.point.z = 0;
// tf2::doTransform(local_origin, global_origin,
// tf2_buffer_.lookupTransform(global_frame_,
// tf2::getFrameId(local_origin),
// tf2::getTimestamp(local_origin)));
tf2::doTransform(local_origin, global_origin,
tf2_buffer_.lookupTransform(global_frame_,
tf2::getFrameId(local_origin),
tf2::getTimestamp(local_origin)));
tf2::convert(global_origin.point, observation_list_.front().origin_);
// make sure to pass on the raytrace/obstacle range of the observation buffer to the observations
@ -117,12 +117,12 @@ void ObservationBuffer::bufferCloud(const sensor_msgs::PointCloud2& cloud)
sensor_msgs::PointCloud2 global_frame_cloud;
// // transform the point cloud
// tf2::doTransform(cloud, global_frame_cloud,
// tf2_buffer_.lookupTransform(global_frame_,
// tf2::getFrameId(cloud),
// tf2::getTimestamp(cloud)));
// global_frame_cloud.header.stamp = cloud.header.stamp;
// transform the point cloud
tf2::doTransform(cloud, global_frame_cloud,
tf2_buffer_.lookupTransform(global_frame_,
tf2::getFrameId(cloud),
tf2::getTimestamp(cloud)));
global_frame_cloud.header.stamp = cloud.header.stamp;
// now we need to remove observations from the cloud that are below or above our height thresholds
sensor_msgs::PointCloud2& observation_cloud = *(observation_list_.front().cloud_);

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