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

update file obstacle_layer and file test plugin

Browse Source
This commit is contained in:
duongtd
2025-11-12 17:38:38 +07:00
parent 19683269c3
commit c94de60a7b
12 changed files with 393 additions and 517 deletions
Download Patch File Download Diff File Expand all files Collapse all files

178
plugins/static_layer.cpp
View File

@@ -5,7 +5,6 @@
#include <costmap_2d/utils.h>
#include <tf2/convert.h>
#include <tf2/utils.h>
// #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <boost/dll/alias.hpp>
@@ -26,55 +25,10 @@ StaticLayer::~StaticLayer()
void StaticLayer::onInitialize()
{
// ros::NodeHandle nh("~/" + name_), g_nh;
current_ = true;
global_frame_ = layered_costmap_->getGlobalFrameID();
getParams();
// nh.param("map_topic", map_topic_, std::string("map"));
// nh.param("first_map_only", first_map_only_, false);
// nh.param("subscribe_to_updates", subscribe_to_updates_, false);
// nh.param("track_unknown_space", track_unknown_space_, true);
// nh.param("use_maximum", use_maximum_, false);
// int temp_lethal_threshold, temp_unknown_cost_value;
// nh.param("lethal_cost_threshold", temp_lethal_threshold, int(100));
// nh.param("unknown_cost_value", temp_unknown_cost_value, int(-1));
// nh.param("trinary_costmap", trinary_costmap_, true);
// nh.param("base_frame_id", base_frame_id_, std::string("base_footprint"));
// lethal_threshold_ = std::max(std::min(temp_lethal_threshold, 100), 0);
// unknown_cost_value_ = temp_unknown_cost_value;
// Only resubscribe if topic has changed
// if (map_sub_.getTopic() != ros::names::resolve(map_topic_))
// {
// // we'll subscribe to the latched topic that the map server uses
// printf("Requesting the map...");
// map_sub_ = g_nh.subscribe(map_topic_, 1, &StaticLayer::incomingMap, this);
// map_received_ = false;
// has_updated_data_ = false;
// ros::Rate r(10);
// while (!map_received_ && g_nh.ok())
// {
// ros::spinOnce();
// r.sleep();
// }
// printf("Received a %d X %d map at %f m/pix", getSizeInCellsX(), getSizeInCellsY(), getResolution());
// if (subscribe_to_updates_)
// {
std::cout<<"Subscribing to updates"<<std::endl;
// map_update_sub_ = g_nh.subscribe(map_topic_ + "_updates", 10, &StaticLayer::incomingUpdate, this);
// }
// }
// else
// {
// has_updated_data_ = true;
// }
}
@@ -84,6 +38,7 @@ bool StaticLayer::getParams()
YAML::Node config = YAML::LoadFile("/home/duongtd/robotics_core/costmap_2d/config/config.yaml");
YAML::Node layer = config["static_layer"];
bool enabled = loadParam(layer, "enabled", true);
bool first_map_only = loadParam(layer, "first_map_only", false);
bool subscribe_to_updates = loadParam(layer, "subscribe_to_updates", false);
@@ -120,18 +75,6 @@ bool StaticLayer::getParams()
}
// void StaticLayer::reconfigureCB(costmap_2d::GenericPluginConfig &config, uint32_t level)
// {
// if (config.enabled != enabled_)
// {
// enabled_ = config.enabled;
// has_updated_data_ = true;
// x_ = y_ = 0;
// width_ = size_x_;
// height_ = size_y_;
// }
// }
void StaticLayer::matchSize()
{
// If we are using rolling costmap, the static map size is
@@ -160,18 +103,20 @@ unsigned char StaticLayer::interpretValue(unsigned char value)
return scale * LETHAL_OBSTACLE;
}
void StaticLayer::handleImpl(const void* data, const std::type_info& type)
void StaticLayer::handleImpl(const void* data,
const std::type_info& type,
const std::string& topic)
{
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));
if (type == typeid(nav_msgs::OccupancyGrid) && topic == "map") {
incomingMap(*static_cast<const nav_msgs::OccupancyGrid*>(data));
} else if (type == typeid(map_msgs::OccupancyGridUpdate) && topic == "map_update") {
incomingUpdate(*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)
void StaticLayer::incomingMap(const nav_msgs::OccupancyGrid& new_map)
{
if(!map_shutdown_)
{
@@ -221,25 +166,13 @@ void StaticLayer::process(const nav_msgs::OccupancyGrid& new_map)
}
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
{
@@ -253,7 +186,8 @@ void StaticLayer::process(const nav_msgs::OccupancyGrid& new_map)
// map_sub_.shutdown();
}
}
void StaticLayer::process(const map_msgs::OccupancyGridUpdate& update)
void StaticLayer::incomingUpdate(const map_msgs::OccupancyGridUpdate& update)
{
if(!map_update_shutdown_)
{
@@ -276,94 +210,10 @@ void StaticLayer::process(const map_msgs::OccupancyGridUpdate& update)
}
}
void StaticLayer::incomingMap(const nav_msgs::OccupancyGrid& new_map)
{
if(!map_shutdown_)
{
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", 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", 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", 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;
// 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;
}
// 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::incomingUpdate(const map_msgs::OccupancyGridUpdate& update)
{
if(!map_update_shutdown_)
{
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::activate()
{
map_shutdown_ = false;
map_update_shutdown_ = false;
onInitialize();
}