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

them file test static_layer

Browse Source
This commit is contained in:
duongtd
2025-11-07 17:44:42 +07:00
parent 79e706b798
commit 498b606e15
148 changed files with 6363 additions and 1599 deletions
Download Patch File Download Diff File Expand all files Collapse all files

201
plugins/static_layer.cpp
View File

@@ -1,11 +1,14 @@
#include <costmap_2d/static_layer.h>
#include <costmap_2d/costmap_math.h>
#include <costmap_2d/data_convert.h>
#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>
using costmap_2d::NO_INFORMATION;
using costmap_2d::LETHAL_OBSTACLE;
using costmap_2d::FREE_SPACE;
@@ -27,6 +30,7 @@ void StaticLayer::onInitialize()
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);
@@ -59,11 +63,11 @@ void StaticLayer::onInitialize()
// r.sleep();
// }
// printf("Received a %d X %d map at %f m/pix", getSizeInCellsX(), getSizeInCellsY(), getResolution());
// printf("Received a %d X %d map at %f m/pix", getSizeInCellsX(), getSizeInCellsY(), getResolution());
// if (subscribe_to_updates_)
// {
// printf("Subscribing to updates");
std::cout<<"Subscribing to updates"<<std::endl;
// map_update_sub_ = g_nh.subscribe(map_topic_ + "_updates", 10, &StaticLayer::incomingUpdate, this);
// }
// }
@@ -74,6 +78,48 @@ void StaticLayer::onInitialize()
}
bool StaticLayer::getParams()
{
try {
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);
bool track_unknown_space = loadParam(layer, "track_unknown_space", true);
bool use_maximum = loadParam(layer, "use_maximum", false);
int lethal_cost_threshold = loadParam(layer, "lethal_cost_threshold", 100);
int unknown_cost_value = loadParam(layer, "unknown_cost_value", -1);
bool trinary_costmap = loadParam(layer, "trinary_costmap", true);
std::string base_frame_id = loadParam(layer, "base_frame_id", std::string("map"));
first_map_only_ = first_map_only;
subscribe_to_updates_ = subscribe_to_updates;
track_unknown_space_ = track_unknown_space;
use_maximum_ = use_maximum;
lethal_threshold_ = std::max(std::min(lethal_cost_threshold, 100), 0);
unknown_cost_value_ = unknown_cost_value;
trinary_costmap_ = trinary_costmap;
base_frame_id_ = base_frame_id;
if (enabled_ != enabled) {
enabled_ = enabled;
has_updated_data_ = true;
x_ = y_ = 0;
width_ = size_x_;
height_ = size_y_;
}
}
catch (const YAML::BadFile& e) {
std::cerr << "Cannot open YAML file: " << e.what() << std::endl;
return false;
}
return true;
}
// void StaticLayer::reconfigureCB(costmap_2d::GenericPluginConfig &config, uint32_t level)
// {
// if (config.enabled != enabled_)
@@ -116,82 +162,88 @@ unsigned char StaticLayer::interpretValue(unsigned char value)
void StaticLayer::incomingMap(const nav_msgs::OccupancyGrid& new_map)
{
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))
if(!map_shutdown_)
{
// 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 size_x = new_map.info.width, size_y = new_map.info.height;
unsigned int index = 0;
printf("Received a %d X %d map at %f m/pix", size_x, size_y, new_map.info.resolution);
// initialize the costmap with static data
for (unsigned int i = 0; i < size_y; ++i)
{
for (unsigned int j = 0; j < size_x; ++j)
// 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))
{
unsigned char value = new_map.data[index];
costmap_[index] = interpretValue(value);
++index;
// 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;
}
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)
{
unsigned int di = 0;
for (unsigned int y = 0; y < update.height ; y++)
if(!map_update_shutdown_)
{
unsigned int index_base = (update.y + y) * size_x_;
for (unsigned int x = 0; x < update.width ; x++)
unsigned int di = 0;
for (unsigned int y = 0; y < update.height ; y++)
{
unsigned int index = index_base + x + update.x;
costmap_[index] = interpretValue(update.data[di++]);
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;
}
x_ = update.x;
y_ = update.y;
width_ = update.width;
height_ = update.height;
has_updated_data_ = true;
}
void StaticLayer::activate()
@@ -199,23 +251,26 @@ void StaticLayer::activate()
onInitialize();
}
// void StaticLayer::deactivate()
// {
// map_sub_.shutdown();
// if (subscribe_to_updates_)
// map_update_sub_.shutdown();
// }
void StaticLayer::deactivate()
{
map_shutdown_ = true;
if (subscribe_to_updates_)
map_update_shutdown_ = true;
}
void StaticLayer::reset()
{
if (first_map_only_)
{
map_shutdown_ = false;
map_update_shutdown_ = false;
has_updated_data_ = true;
}
else
{
onInitialize();
}
printf("RESET MAP");
}
void StaticLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,
@@ -261,15 +316,16 @@ void StaticLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int
unsigned int mx, my;
double wx, wy;
// Might even be in a different frame
geometry_msgs::TransformStamped transform;
// geometry_msgs::TransformStamped transform;
tf2::TransformStampedMsg transformMsg;
try
{
// transformMsg = tf_->lookupTransform(map_frame_, global_frame_, ros::Time(0));
tf_->canTransform(map_frame_, global_frame_, tf2::Time());
tf_->lookupTransform(map_frame_,
global_frame_,
tf2::Time());
bool status =tf_->canTransform(map_frame_, global_frame_, tf2::Time());
if(!status) throw tf2::TransformException("[static_layer] Cannot transform");
transformMsg = tf_->lookupTransform(map_frame_,
global_frame_,
tf2::Time());
}
catch (tf2::TransformException ex)
{
@@ -278,7 +334,8 @@ void StaticLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int
}
// Copy map data given proper transformations
tf2::Transform tf2_transform;
// tf2::convert(transform.transform, tf2_transform);
tf2_transform = convertToTf2Transform(transformMsg.transform);
// tf2::convert(transformMsg.transform, tf2_transform);
for (unsigned int i = min_i; i < max_i; ++i)
{
for (unsigned int j = min_j; j < max_j; ++j)
@@ -287,7 +344,7 @@ void StaticLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int
layered_costmap_->getCostmap()->mapToWorld(i, j, wx, wy);
// Transform from global_frame_ to map_frame_
tf2::Vector3 p(wx, wy, 0);
// p = tf2_transform*p;
p = tf2_transform*p;
// Set master_grid with cell from map
if (worldToMap(p.x(), p.y(), mx, my))
{
@@ -302,7 +359,7 @@ void StaticLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int
}
// Export factory function
static PluginPtr create_static_plugin() {
static PluginCostmapLayerPtr create_static_plugin() {
return std::make_shared<StaticLayer>();
}