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

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,8 +18,10 @@ 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());
@@ -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_)