void InflationLayer::onFootprintChanged()
{
inscribed_radius_ = layered_costmap_->getInscribedRadius();
cell_inflation_radius_ = cellDistance( inflation_radius_ );
computeCaches();
need_reinflation_ = true;
ROS_DEBUG( "InflationLayer::onFootprintChanged(): num footprint points: %lu, inscribed_radius_ = %.3f, inflation_radius_ = %.3f",
layered_costmap_->getFootprint().size(), inscribed_radius_, inflation_radius_ );
}
void InflationLayer::matchSize()
{
costmap_2d::Costmap2D* costmap = layered_costmap_->getCostmap();
resolution_ = costmap->getResolution();
cell_inflation_radius_ = cellDistance(inflation_radius_);
computeCaches();
unsigned int size_x = costmap->getSizeInCellsX(), size_y = costmap->getSizeInCellsY();
if (seen_)
delete seen_;
seen_ = new bool[size_x * size_y];
}
void InflationLayer::matchSize()
{
boost::unique_lock < boost::recursive_mutex > lock(*inflation_access_);
costmap_2d::Costmap2D* costmap = layered_costmap_->getCostmap();
resolution_ = costmap->getResolution();
cell_inflation_radius_ = cellDistance(inflation_radius_);
computeCaches();
unsigned int size_x = costmap->getSizeInCellsX(), size_y = costmap->getSizeInCellsY();
if (seen_)
delete[] seen_;
seen_size_ = size_x * size_y;
seen_ = new bool[seen_size_];
}
void InflationLayer::setInflationParameters(double inflation_radius, double cost_scaling_factor)
{
if (weight_ != cost_scaling_factor || inflation_radius_ != inflation_radius)
{
// Lock here so that reconfiguring the inflation radius doesn't cause segfaults
// when accessing the cached arrays
boost::unique_lock < boost::recursive_mutex > lock(*inflation_access_);
inflation_radius_ = inflation_radius;
cell_inflation_radius_ = cellDistance(inflation_radius_);
weight_ = cost_scaling_factor;
need_reinflation_ = true;
computeCaches();
}
}
void InflationLayer::reconfigureCB(costmap_2d::InflationPluginConfig &config, uint32_t level)
{
if (weight_ != config.cost_scaling_factor || inflation_radius_ != config.inflation_radius)
{
inflation_radius_ = config.inflation_radius;
cell_inflation_radius_ = cellDistance(inflation_radius_);
weight_ = config.cost_scaling_factor;
need_reinflation_ = true;
computeCaches();
}
if (enabled_ != config.enabled) {
enabled_ = config.enabled;
need_reinflation_ = true;
}
}
void Costmap2D::raytraceFreespace(const Observation& clearing_observation){
//create the functor that we'll use to clear cells from the costmap
ClearCell clearer(costmap_);
double ox = clearing_observation.origin_.x;
double oy = clearing_observation.origin_.y;
sensor_msgs::PointCloud cloud = clearing_observation.cloud_;
//get the map coordinates of the origin of the sensor
unsigned int x0, y0;
if(!worldToMap(ox, oy, x0, y0)){
ROS_WARN("The origin for the sensor at (%.2f, %.2f) is out of map bounds. So, the costmap cannot raytrace for it.", ox, oy);
return;
}
//we can pre-compute the enpoints of the map outside of the inner loop... we'll need these later
double map_end_x = origin_x_ + getSizeInMetersX();
double map_end_y = origin_y_ + getSizeInMetersY();
//for each point in the cloud, we want to trace a line from the origin and clear obstacles along it
for(unsigned int i = 0; i < cloud.points.size(); ++i){
double wx = cloud.points[i].x;
double wy = cloud.points[i].y;
//now we also need to make sure that the enpoint we're raytracing
//to isn't off the costmap and scale if necessary
double a = wx - ox;
double b = wy - oy;
//the minimum value to raytrace from is the origin
if(wx < origin_x_){
double t = (origin_x_ - ox) / a;
wx = origin_x_;
wy = oy + b * t;
}
if(wy < origin_y_){
double t = (origin_y_ - oy) / b;
wx = ox + a * t;
wy = origin_y_;
}
//the maximum value to raytrace to is the end of the map
if(wx > map_end_x){
double t = (map_end_x - ox) / a;
wx = map_end_x;
wy = oy + b * t;
}
if(wy > map_end_y){
double t = (map_end_y - oy) / b;
wx = ox + a * t;
wy = map_end_y;
}
//now that the vector is scaled correctly... we'll get the map coordinates of its endpoint
unsigned int x1, y1;
//check for legality just in case
if(!worldToMap(wx, wy, x1, y1))
continue;
unsigned int cell_raytrace_range = cellDistance(clearing_observation.raytrace_range_);
//and finally... we can execute our trace to clear obstacles along that line
raytraceLine(clearer, x0, y0, x1, y1, cell_raytrace_range);
}
}
Costmap2D::Costmap2D(unsigned int cells_size_x, unsigned int cells_size_y,
double resolution, double origin_x, double origin_y, double inscribed_radius,
double circumscribed_radius, double inflation_radius, double max_obstacle_range,
double max_obstacle_height, double max_raytrace_range, double weight,
const std::vector<unsigned char>& static_data, unsigned char lethal_threshold, bool track_unknown_space, unsigned char unknown_cost_value) : size_x_(cells_size_x),
size_y_(cells_size_y), resolution_(resolution), origin_x_(origin_x), origin_y_(origin_y), static_map_(NULL),
costmap_(NULL), markers_(NULL), max_obstacle_range_(max_obstacle_range),
max_obstacle_height_(max_obstacle_height), max_raytrace_range_(max_raytrace_range), cached_costs_(NULL), cached_distances_(NULL),
inscribed_radius_(inscribed_radius), circumscribed_radius_(circumscribed_radius), inflation_radius_(inflation_radius),
weight_(weight), lethal_threshold_(lethal_threshold), track_unknown_space_(track_unknown_space), unknown_cost_value_(unknown_cost_value), inflation_queue_(){
//creat the costmap, static_map, and markers
costmap_ = new unsigned char[size_x_ * size_y_];
static_map_ = new unsigned char[size_x_ * size_y_];
markers_ = new unsigned char[size_x_ * size_y_];
memset(markers_, 0, size_x_ * size_y_ * sizeof(unsigned char));
//convert our inflations from world to cell distance
cell_inscribed_radius_ = cellDistance(inscribed_radius);
cell_circumscribed_radius_ = cellDistance(circumscribed_radius);
cell_inflation_radius_ = cellDistance(inflation_radius);
//set the cost for the circumscribed radius of the robot
circumscribed_cost_lb_ = computeCost(cell_circumscribed_radius_);
//based on the inflation radius... compute distance and cost caches
cached_costs_ = new unsigned char*[cell_inflation_radius_ + 2];
cached_distances_ = new double*[cell_inflation_radius_ + 2];
for(unsigned int i = 0; i <= cell_inflation_radius_ + 1; ++i){
cached_costs_[i] = new unsigned char[cell_inflation_radius_ + 2];
cached_distances_[i] = new double[cell_inflation_radius_ + 2];
for(unsigned int j = 0; j <= cell_inflation_radius_ + 1; ++j){
cached_distances_[i][j] = sqrt(i*i + j*j);
cached_costs_[i][j] = computeCost(cached_distances_[i][j]);
}
}
if(!static_data.empty()){
ROS_ASSERT_MSG(size_x_ * size_y_ == static_data.size(), "If you want to initialize a costmap with static data, their sizes must match.");
//make sure the inflation queue is empty at the beginning of the cycle (should always be true)
ROS_ASSERT_MSG(inflation_queue_.empty(), "The inflation queue must be empty at the beginning of inflation");
unsigned int index = 0;
unsigned char* costmap_index = costmap_;
std::vector<unsigned char>::const_iterator static_data_index = static_data.begin();
//initialize the costmap with static data
for(unsigned int i = 0; i < size_y_; ++i){
for(unsigned int j = 0; j < size_x_; ++j){
//check if the static value is above the unknown or lethal thresholds
if(track_unknown_space_ && unknown_cost_value_ > 0 && *static_data_index == unknown_cost_value_)
*costmap_index = NO_INFORMATION;
else if(*static_data_index >= lethal_threshold_)
*costmap_index = LETHAL_OBSTACLE;
else
*costmap_index = FREE_SPACE;
if(*costmap_index == LETHAL_OBSTACLE){
unsigned int mx, my;
indexToCells(index, mx, my);
enqueue(index, mx, my, mx, my, inflation_queue_);
}
++costmap_index;
++static_data_index;
++index;
}
}
//now... let's inflate the obstacles
inflateObstacles(inflation_queue_);
//we also want to keep a copy of the current costmap as the static map
memcpy(static_map_, costmap_, size_x_ * size_y_ * sizeof(unsigned char));
}
else{
//everything is unknown initially if we don't have a static map unless we aren't tracking unkown space in which case it is free
resetMaps();
}
}
void VoxelLayer::raytraceFreespace(const Observation& clearing_observation, double* min_x, double* min_y,
double* max_x, double* max_y)
{
if (clearing_observation.cloud_->points.size() == 0)
return;
double sensor_x, sensor_y, sensor_z;
double ox = clearing_observation.origin_.x;
double oy = clearing_observation.origin_.y;
double oz = clearing_observation.origin_.z;
if (!worldToMap3DFloat(ox, oy, oz, sensor_x, sensor_y, sensor_z))
{
ROS_WARN_THROTTLE(
1.0,
"The origin for the sensor at (%.2f, %.2f, %.2f) is out of map bounds. So, the costmap cannot raytrace for it.",
ox, oy, oz);
return;
}
bool publish_clearing_points = (clearing_endpoints_pub_.getNumSubscribers() > 0);
if( publish_clearing_points )
{
clearing_endpoints_.points.clear();
clearing_endpoints_.points.reserve( clearing_observation.cloud_->points.size() );
}
//we can pre-compute the enpoints of the map outside of the inner loop... we'll need these later
double map_end_x = origin_x_ + getSizeInMetersX();
double map_end_y = origin_y_ + getSizeInMetersY();
for (unsigned int i = 0; i < clearing_observation.cloud_->points.size(); ++i)
{
double wpx = clearing_observation.cloud_->points[i].x;
double wpy = clearing_observation.cloud_->points[i].y;
double wpz = clearing_observation.cloud_->points[i].z;
double distance = dist(ox, oy, oz, wpx, wpy, wpz);
double scaling_fact = 1.0;
scaling_fact = std::max(std::min(scaling_fact, (distance - 2 * resolution_) / distance), 0.0);
wpx = scaling_fact * (wpx - ox) + ox;
wpy = scaling_fact * (wpy - oy) + oy;
wpz = scaling_fact * (wpz - oz) + oz;
double a = wpx - ox;
double b = wpy - oy;
double c = wpz - oz;
double t = 1.0;
//we can only raytrace to a maximum z height
if (wpz > max_obstacle_height_)
{
//we know we want the vector's z value to be max_z
t = std::max(0.0, std::min(t, (max_obstacle_height_ - 0.01 - oz) / c));
}
//and we can only raytrace down to the floor
else if (wpz < origin_z_)
{
//we know we want the vector's z value to be 0.0
t = std::min(t, (origin_z_ - oz) / c);
}
//the minimum value to raytrace from is the origin
if (wpx < origin_x_)
{
t = std::min(t, (origin_x_ - ox) / a);
}
if (wpy < origin_y_)
{
t = std::min(t, (origin_y_ - oy) / b);
}
//the maximum value to raytrace to is the end of the map
if (wpx > map_end_x)
{
t = std::min(t, (map_end_x - ox) / a);
}
if (wpy > map_end_y)
{
t = std::min(t, (map_end_y - oy) / b);
}
wpx = ox + a * t;
wpy = oy + b * t;
wpz = oz + c * t;
double point_x, point_y, point_z;
if (worldToMap3DFloat(wpx, wpy, wpz, point_x, point_y, point_z))
{
unsigned int cell_raytrace_range = cellDistance(clearing_observation.raytrace_range_);
//voxel_grid_.markVoxelLine(sensor_x, sensor_y, sensor_z, point_x, point_y, point_z);
voxel_grid_.clearVoxelLineInMap(sensor_x, sensor_y, sensor_z, point_x, point_y, point_z, costmap_,
unknown_threshold_, mark_threshold_, FREE_SPACE, NO_INFORMATION,
cell_raytrace_range);
updateRaytraceBounds(ox, oy, wpx, wpy, clearing_observation.raytrace_range_, min_x, min_y, max_x, max_y);
if( publish_clearing_points )
{
geometry_msgs::Point32 point;
point.x = wpx;
point.y = wpy;
point.z = wpz;
clearing_endpoints_.points.push_back( point );
}
}
}
if( publish_clearing_points )
{
clearing_endpoints_.header.frame_id = global_frame_;
clearing_endpoints_.header.stamp = pcl_conversions::fromPCL(clearing_observation.cloud_->header).stamp;
clearing_endpoints_.header.seq = clearing_observation.cloud_->header.seq;
clearing_endpoints_pub_.publish( clearing_endpoints_ );
}
}
