353 lines
11 KiB
C++
353 lines
11 KiB
C++
|
|
#include <costmap_2d/static_layer.h>
|
|
#include <costmap_2d/costmap_math.h>
|
|
#include <costmap_2d/data_convert.h>
|
|
#include <costmap_2d/utils.h>
|
|
#include <tf3/convert.h>
|
|
#include <tf3/utils.h>
|
|
#include <boost/dll/alias.hpp>
|
|
#include <filesystem>
|
|
#include <string>
|
|
#include <iostream>
|
|
|
|
using costmap_2d::NO_INFORMATION;
|
|
using costmap_2d::LETHAL_OBSTACLE;
|
|
using costmap_2d::FREE_SPACE;
|
|
|
|
namespace costmap_2d
|
|
{
|
|
|
|
StaticLayer::StaticLayer()
|
|
{
|
|
threshold_ = &lethal_threshold_;
|
|
}
|
|
|
|
StaticLayer::~StaticLayer()
|
|
{}
|
|
|
|
void StaticLayer::onInitialize()
|
|
{
|
|
current_ = true;
|
|
|
|
global_frame_ = layered_costmap_->getGlobalFrameID();
|
|
getParams();
|
|
|
|
}
|
|
|
|
bool StaticLayer::getParams()
|
|
{
|
|
try {
|
|
std::string config_file_name = "config.yaml";
|
|
std::string folder = COSTMAP_2D_DIR;
|
|
|
|
|
|
std::string path_source = getSourceFile(folder,config_file_name);
|
|
if(path_source != " ")
|
|
{
|
|
std::cout << "Path source: " << path_source << std::endl;
|
|
}
|
|
else
|
|
{
|
|
std::cout << "/cfg folder not found!" << std::endl;
|
|
}
|
|
|
|
|
|
YAML::Node config = YAML::LoadFile(path_source);
|
|
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::matchSize()
|
|
{
|
|
// If we are using rolling costmap, the static map size is
|
|
// unrelated to the size of the layered costmap
|
|
if (!layered_costmap_->isRolling())
|
|
{
|
|
Costmap2D* master = layered_costmap_->getCostmap();
|
|
resizeMap(master->getSizeInCellsX(), master->getSizeInCellsY(), master->getResolution(),
|
|
master->getOriginX(), master->getOriginY());
|
|
}
|
|
}
|
|
|
|
unsigned char StaticLayer::interpretValue(unsigned char value)
|
|
{
|
|
// check if the static value is above the unknown or lethal thresholds
|
|
if (track_unknown_space_ && value == unknown_cost_value_)
|
|
return NO_INFORMATION;
|
|
else if (!track_unknown_space_ && value == unknown_cost_value_)
|
|
return FREE_SPACE;
|
|
else if (value >= lethal_threshold_)
|
|
return LETHAL_OBSTACLE;
|
|
else if (trinary_costmap_)
|
|
return FREE_SPACE;
|
|
|
|
double scale = (double) value / lethal_threshold_;
|
|
return scale * LETHAL_OBSTACLE;
|
|
}
|
|
|
|
void StaticLayer::handleImpl(const void* data,
|
|
const std::type_info& type,
|
|
const std::string& topic)
|
|
{
|
|
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::incomingMap(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;
|
|
|
|
// 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::incomingUpdate(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::activate()
|
|
{
|
|
map_shutdown_ = false;
|
|
map_update_shutdown_ = false;
|
|
onInitialize();
|
|
}
|
|
|
|
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,
|
|
double* max_x, double* max_y)
|
|
{
|
|
|
|
if( !layered_costmap_->isRolling() ){
|
|
if (!map_received_ || !(has_updated_data_ || has_extra_bounds_))
|
|
return;
|
|
}
|
|
|
|
useExtraBounds(min_x, min_y, max_x, max_y);
|
|
|
|
double wx, wy;
|
|
|
|
mapToWorld(x_, y_, wx, wy);
|
|
*min_x = std::min(wx, *min_x);
|
|
*min_y = std::min(wy, *min_y);
|
|
|
|
mapToWorld(x_ + width_, y_ + height_, wx, wy);
|
|
*max_x = std::max(wx, *max_x);
|
|
*max_y = std::max(wy, *max_y);
|
|
|
|
has_updated_data_ = false;
|
|
}
|
|
|
|
void StaticLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
|
|
{
|
|
if (!map_received_)
|
|
return;
|
|
|
|
if (!layered_costmap_->isRolling())
|
|
{
|
|
// if not rolling, the layered costmap (master_grid) has same coordinates as this layer
|
|
if (!use_maximum_)
|
|
updateWithOverwrite(master_grid, min_i, min_j, max_i, max_j);
|
|
else
|
|
updateWithMax(master_grid, min_i, min_j, max_i, max_j);
|
|
}
|
|
else
|
|
{
|
|
// If rolling window, the master_grid is unlikely to have same coordinates as this layer
|
|
unsigned int mx, my;
|
|
double wx, wy;
|
|
// Might even be in a different frame
|
|
// geometry_msgs::TransformStamped transform;
|
|
tf3::TransformStampedMsg transformMsg;
|
|
try
|
|
{
|
|
// transformMsg = tf_->lookupTransform(map_frame_, global_frame_, ros::Time(0));
|
|
bool status =tf_->canTransform(map_frame_, global_frame_, tf3::Time());
|
|
if(!status) throw tf3::TransformException("[static_layer] Cannot transform");
|
|
transformMsg = tf_->lookupTransform(map_frame_,
|
|
global_frame_,
|
|
tf3::Time());
|
|
}
|
|
catch (tf3::TransformException ex)
|
|
{
|
|
printf("%s", ex.what());
|
|
return;
|
|
}
|
|
// Copy map data given proper transformations
|
|
tf3::Transform tf3_transform;
|
|
tf3_transform = convertTotf3Transform(transformMsg.transform);
|
|
// tf3::convert(transformMsg.transform, tf3_transform);
|
|
for (unsigned int i = min_i; i < max_i; ++i)
|
|
{
|
|
for (unsigned int j = min_j; j < max_j; ++j)
|
|
{
|
|
// Convert master_grid coordinates (i,j) into global_frame_(wx,wy) coordinates
|
|
layered_costmap_->getCostmap()->mapToWorld(i, j, wx, wy);
|
|
// Transform from global_frame_ to map_frame_
|
|
tf3::Vector3 p(wx, wy, 0);
|
|
p = tf3_transform*p;
|
|
// Set master_grid with cell from map
|
|
if (worldToMap(p.x(), p.y(), mx, my))
|
|
{
|
|
if (!use_maximum_)
|
|
master_grid.setCost(i, j, getCost(mx, my));
|
|
else
|
|
master_grid.setCost(i, j, std::max(getCost(mx, my), master_grid.getCost(i, j)));
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
// Export factory function
|
|
static PluginLayerPtr create_static_plugin() {
|
|
return std::make_shared<StaticLayer>();
|
|
}
|
|
|
|
// Alias cho Boost.DLL (nếu muốn dùng boost::dll::import_alias)
|
|
BOOST_DLL_ALIAS(create_static_plugin, create_plugin)
|
|
|
|
} // namespace costmap_2d
|