313 lines
9.3 KiB
C++
313 lines
9.3 KiB
C++
|
|
#include <costmap_2d/static_layer.h>
|
|
#include <costmap_2d/costmap_math.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;
|
|
|
|
namespace costmap_2d
|
|
{
|
|
|
|
StaticLayer::StaticLayer()
|
|
{
|
|
threshold_ = &lethal_threshold_;
|
|
}
|
|
|
|
StaticLayer::~StaticLayer()
|
|
{}
|
|
|
|
void StaticLayer::onInitialize()
|
|
{
|
|
// ros::NodeHandle nh("~/" + name_), g_nh;
|
|
current_ = true;
|
|
|
|
global_frame_ = layered_costmap_->getGlobalFrameID();
|
|
// 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_)
|
|
// {
|
|
// printf("Subscribing to updates");
|
|
// map_update_sub_ = g_nh.subscribe(map_topic_ + "_updates", 10, &StaticLayer::incomingUpdate, this);
|
|
// }
|
|
// }
|
|
// else
|
|
// {
|
|
// has_updated_data_ = true;
|
|
// }
|
|
|
|
}
|
|
|
|
// 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
|
|
// 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::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))
|
|
{
|
|
// 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_sub_.shutdown();
|
|
}
|
|
}
|
|
|
|
void StaticLayer::incomingUpdate(const map_msgs::OccupancyGridUpdate& update)
|
|
{
|
|
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()
|
|
{
|
|
onInitialize();
|
|
}
|
|
|
|
// void StaticLayer::deactivate()
|
|
// {
|
|
// map_sub_.shutdown();
|
|
// if (subscribe_to_updates_)
|
|
// map_update_sub_.shutdown();
|
|
// }
|
|
|
|
void StaticLayer::reset()
|
|
{
|
|
if (first_map_only_)
|
|
{
|
|
has_updated_data_ = true;
|
|
}
|
|
else
|
|
{
|
|
onInitialize();
|
|
}
|
|
}
|
|
|
|
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;
|
|
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());
|
|
}
|
|
catch (tf2::TransformException ex)
|
|
{
|
|
printf("%s", ex.what());
|
|
return;
|
|
}
|
|
// Copy map data given proper transformations
|
|
tf2::Transform tf2_transform;
|
|
// tf2::convert(transform.transform, tf2_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_
|
|
tf2::Vector3 p(wx, wy, 0);
|
|
// p = tf2_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 PluginPtr 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
|