inflation_layer.cpp

// Copyright (c) 2026, Dexory (Tony Najjar)
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
 
#include "nav2_costmap_2d/inflation_layer.hpp"
 
#include <limits>
#include <map>
#include <vector>
#include <algorithm>
#include <utility>
#include <cmath>
 
#ifdef _OPENMP
#include <omp.h>
#endif
 
#include "nav2_costmap_2d/costmap_math.hpp"
#include "nav2_costmap_2d/footprint.hpp"
#include "nav2_costmap_2d/distance_transform.hpp"
#include "pluginlib/class_list_macros.hpp"
#include "rclcpp/parameter_events_filter.hpp"
 
PLUGINLIB_EXPORT_CLASS(nav2_costmap_2d::InflationLayer, nav2_costmap_2d::Layer)
 
using nav2_costmap_2d::LETHAL_OBSTACLE;
using nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE;
using nav2_costmap_2d::NO_INFORMATION;
using rcl_interfaces::msg::ParameterType;
 
namespace nav2_costmap_2d
{
 
InflationLayer::InflationLayer()
: inflation_radius_(0),
  inscribed_radius_(0),
  custom_inscribed_radius_(-1.0),
  cost_scaling_factor_(0),
  inflate_unknown_(false),
  inflate_around_unknown_(false),
  cell_inflation_radius_(0),
  num_threads_(-1),
  resolution_(0),
  last_min_x_(std::numeric_limits<double>::lowest()),
  last_min_y_(std::numeric_limits<double>::lowest()),
  last_max_x_(std::numeric_limits<double>::max()),
  last_max_y_(std::numeric_limits<double>::max())
{
  access_ = new mutex_t();
}
 
InflationLayer::~InflationLayer()
{
  delete access_;
}
 
void
InflationLayer::updateInscribedRadius()
{
  if (custom_inscribed_radius_ >= 0.0) {
    RCLCPP_WARN(
      logger_,
      "POTENTIAL SAFETY ISSUE!!! Make sure you fully understand the consequences of changing the "
      "inscribed radius! Inflation layer is set to use a custom inscribed radius of %.3f m instead "
      "of the footprint's inscribed radius of %.3f m. This can have serious implications on the "
      "robot's safety and is not recommended unless specifically needed. This practice is intended "
      "only for controllers that are customized explicitly to feed on such data. It is NOT "
      "intended for global path planners or setups that depend on the footprint inscribed radius!",
      custom_inscribed_radius_, layered_costmap_->getInscribedRadius());
    inscribed_radius_ = custom_inscribed_radius_;
  } else {
    inscribed_radius_ = layered_costmap_->getInscribedRadius();
  }
}
 
void
InflationLayer::onInitialize()
{
  {
    auto node = node_.lock();
    if (!node) {
      throw std::runtime_error{"Failed to lock node"};
    }
    enabled_ = node->declare_or_get_parameter(name_ + "." + "enabled", true);
    inflation_radius_ = node->declare_or_get_parameter(name_ + "." + "inflation_radius", 0.55);
    custom_inscribed_radius_ = node->declare_or_get_parameter(
      name_ + "." + "custom_inscribed_radius", -1.0);
    cost_scaling_factor_ = node->declare_or_get_parameter(
      name_ + "." + "cost_scaling_factor", 10.0);
    inflate_unknown_ = node->declare_or_get_parameter(name_ + "." + "inflate_unknown", false);
    inflate_around_unknown_ = node->declare_or_get_parameter(
      name_ + "." + "inflate_around_unknown", false);
    num_threads_ = node->declare_or_get_parameter(
      name_ + "." + "num_threads", -1);
  }
  updateInscribedRadius();
  setCurrent(true);
  need_reinflation_ = false;
  matchSize();
}
 
void InflationLayer::activate()
{
  auto node = node_.lock();
  post_set_params_handler_ = node->add_post_set_parameters_callback(
    std::bind(
      &InflationLayer::updateParametersCallback,
      this, std::placeholders::_1));
  on_set_params_handler_ = node->add_on_set_parameters_callback(
    std::bind(
      &InflationLayer::validateParameterUpdatesCallback,
      this, std::placeholders::_1));
}
 
void InflationLayer::deactivate()
{
  auto node = node_.lock();
  if (post_set_params_handler_ && node) {
    node->remove_post_set_parameters_callback(post_set_params_handler_.get());
  }
  post_set_params_handler_.reset();
  if (on_set_params_handler_ && node) {
    node->remove_on_set_parameters_callback(on_set_params_handler_.get());
  }
  on_set_params_handler_.reset();
}
 
void
InflationLayer::matchSize()
{
  std::lock_guard<Costmap2D::mutex_t> guard(*getMutex());
  nav2_costmap_2d::Costmap2D * costmap = layered_costmap_->getCostmap();
  resolution_ = costmap->getResolution();
  cell_inflation_radius_ = cellDistance(inflation_radius_);
  computeCaches();
}
 
void
InflationLayer::updateBounds(
  double /*robot_x*/, double /*robot_y*/, double /*robot_yaw*/, double * min_x,
  double * min_y, double * max_x, double * max_y)
{
  std::lock_guard<Costmap2D::mutex_t> guard(*getMutex());
  if (need_reinflation_) {
    // Reset last_* to "no expansion" values so the next cycle won't
    // merge with these full-map bounds (avoids double full-map update after reset)
    last_min_x_ = last_min_y_ = std::numeric_limits<double>::max();
    last_max_x_ = last_max_y_ = std::numeric_limits<double>::lowest();
 
    *min_x = std::numeric_limits<double>::lowest();
    *min_y = std::numeric_limits<double>::lowest();
    *max_x = std::numeric_limits<double>::max();
    *max_y = std::numeric_limits<double>::max();
    need_reinflation_ = false;
  } else {
    double tmp_min_x = last_min_x_;
    double tmp_min_y = last_min_y_;
    double tmp_max_x = last_max_x_;
    double tmp_max_y = last_max_y_;
    last_min_x_ = *min_x;
    last_min_y_ = *min_y;
    last_max_x_ = *max_x;
    last_max_y_ = *max_y;
    *min_x = std::min(tmp_min_x, *min_x) - inflation_radius_;
    *min_y = std::min(tmp_min_y, *min_y) - inflation_radius_;
    *max_x = std::max(tmp_max_x, *max_x) + inflation_radius_;
    *max_y = std::max(tmp_max_y, *max_y) + inflation_radius_;
  }
}
 
int
InflationLayer::getOptimalThreadCount()
{
#ifdef _OPENMP
  // If num_threads parameter is explicitly set (> 0), use it
  if (num_threads_ > 0) {
    RCLCPP_INFO_ONCE(
      logger_,
      "OpenMP: Using configured num_threads: %d",
      num_threads_);
    return num_threads_;
  }
 
  // Otherwise use auto-detection: half the available cores for memory-bound algorithms
  // Balances performance with memory bandwidth and safety on constrained systems
  // Respects OMP_NUM_THREADS environment variable
  int cpu_cores = omp_get_max_threads();
  int optimal = std::max(1, cpu_cores / 2);
 
  RCLCPP_INFO_ONCE(
    logger_,
    "OpenMP: %d cores available, using %d threads (auto)",
    cpu_cores, optimal);
 
  return optimal;
#else
  return 1;
#endif
}
 
void
InflationLayer::onFootprintChanged()
{
  std::lock_guard<Costmap2D::mutex_t> guard(*getMutex());
  updateInscribedRadius();
  cell_inflation_radius_ = cellDistance(inflation_radius_);
  computeCaches();
  need_reinflation_ = true;
 
  if (inflation_radius_ < inscribed_radius_) {
    RCLCPP_WARN(
      logger_,
      "The configured inflation radius (%.3f) is smaller than "
      "the computed/custom inscribed radius (%.3f) of your footprint, "
      "it is highly recommended to set inflation radius to be at "
      "least as big as the inscribed radius to avoid collisions",
      inflation_radius_, inscribed_radius_);
  }
 
  RCLCPP_DEBUG(
    logger_, "InflationLayer::onFootprintChanged(): num footprint points: %zu,"
    " inscribed_radius_ = %.3f, inflation_radius_ = %.3f",
    layered_costmap_->getFootprint().size(), inscribed_radius_, inflation_radius_);
}
 
void
InflationLayer::applyInflation(
  unsigned char * master_array,
  const MatrixXfRM & distance_map,
  int min_i, int min_j, int max_i, int max_j,
  int roi_min_i, int roi_min_j,
  unsigned int size_x)
{
  const float cell_inflation_radius_f = static_cast<float>(cell_inflation_radius_);
  const int lut_max = static_cast<int>(cost_lut_.size() - 1);
  const unsigned char * lut_data = cost_lut_.data();
  const int lut_precision = COST_LUT_PRECISION;
  const bool inflate_unk = inflate_unknown_;
 
#ifdef _OPENMP
  const int num_threads = getOptimalThreadCount();
  #pragma omp parallel for num_threads(num_threads) schedule(dynamic, 16)
#endif
  for (int j = min_j; j < max_j; ++j) {
    const int row_offset = j * static_cast<int>(size_x);
    const int dist_row = j - roi_min_j;
 
    for (int i = min_i; i < max_i; ++i) {
      const float distance_cells = distance_map(dist_row, i - roi_min_i);
      if (distance_cells > cell_inflation_radius_f) {
        continue;
      }
 
      const unsigned int index = row_offset + i;
      const unsigned char old_cost = master_array[index];
      const unsigned int d_scaled = std::min(
        static_cast<unsigned int>(lut_max),
        static_cast<unsigned int>(distance_cells * lut_precision + 0.5f));
      const unsigned char cost = lut_data[d_scaled];
 
      if (old_cost == NO_INFORMATION &&
        (inflate_unk ? (cost > FREE_SPACE) : (cost >= INSCRIBED_INFLATED_OBSTACLE)))
      {
        master_array[index] = cost;
      } else {
        master_array[index] = std::max(old_cost, cost);
      }
    }
  }
}
 
void
InflationLayer::updateCosts(
  nav2_costmap_2d::Costmap2D & master_grid, int min_i, int min_j,
  int max_i,
  int max_j)
{
  std::lock_guard<Costmap2D::mutex_t> guard(*getMutex());
  if (!enabled_ || (cell_inflation_radius_ == 0)) {
    return;
  }
 
  unsigned char * master_array = master_grid.getCharMap();
  const unsigned int size_x = master_grid.getSizeInCellsX();
  const unsigned int size_y = master_grid.getSizeInCellsY();
 
  min_i = std::max(0, min_i);
  min_j = std::max(0, min_j);
  max_i = std::min(static_cast<int>(size_x), max_i);
  max_j = std::min(static_cast<int>(size_y), max_j);
 
  // Compute padded ROI bounds for distance transform
  const int padding = static_cast<int>(cell_inflation_radius_);
  int roi_min_i = std::max(0, min_i - padding);
  int roi_min_j = std::max(0, min_j - padding);
  int roi_max_i = std::min(static_cast<int>(size_x), max_i + padding);
  int roi_max_j = std::min(static_cast<int>(size_y), max_j + padding);
 
  const int roi_width = roi_max_i - roi_min_i;
  const int roi_height = roi_max_j - roi_min_j;
 
  // Create distance map: obstacles = 0, free space = INF
  MatrixXfRM distance_map(roi_height, roi_width);
 
  // Initialize mask (parallelized)
#ifdef _OPENMP
  const int num_threads = getOptimalThreadCount();
  #pragma omp parallel for num_threads(num_threads) schedule(dynamic, 16)
#endif
  for (int y = 0; y < roi_height; y++) {
    const int src_y = y + roi_min_j;
    for (int x = 0; x < roi_width; x++) {
      const int src_x = x + roi_min_i;
      const unsigned char cell = master_array[src_y * size_x + src_x];
 
      if (inflate_around_unknown_) {
        // Treat both LETHAL_OBSTACLE and NO_INFORMATION as obstacles
        distance_map(y,
            x) = (cell != LETHAL_OBSTACLE &&
          cell != NO_INFORMATION) ? DistanceTransform::DT_INF : 0.0f;
      } else {
        // Only LETHAL_OBSTACLE is treated as obstacle
        distance_map(y, x) = (cell != LETHAL_OBSTACLE) ? DistanceTransform::DT_INF : 0.0f;
      }
    }
  }
 
  // Perform Felzenszwalb-Huttenlocher distance transform
  DistanceTransform::distanceTransform2D(distance_map, roi_height, roi_width);
 
  // Apply inflation costs
  applyInflation(
    master_array, distance_map,
    min_i, min_j, max_i, max_j,
    roi_min_i, roi_min_j, size_x);
 
  setCurrent(true);
}
 
 
void
InflationLayer::computeCaches()
{
  std::lock_guard<Costmap2D::mutex_t> guard(*getMutex());
  if (cell_inflation_radius_ == 0) {
    return;
  }
 
  // Generate cost lookup table for distance -> cost mapping
  const unsigned int max_dist_scaled = cell_inflation_radius_ * COST_LUT_PRECISION + 1;
  cost_lut_.resize(max_dist_scaled + 1);
  for (unsigned int d_scaled = 0; d_scaled <= max_dist_scaled; ++d_scaled) {
    const double distance = static_cast<double>(d_scaled) / COST_LUT_PRECISION;
    cost_lut_[d_scaled] = computeCost(distance);
  }
}
 
 
rcl_interfaces::msg::SetParametersResult InflationLayer::validateParameterUpdatesCallback(
  const std::vector<rclcpp::Parameter> & parameters)
{
  rcl_interfaces::msg::SetParametersResult result;
  result.successful = true;
  for (const auto & parameter : parameters) {
    const auto & param_type = parameter.get_type();
    const auto & param_name = parameter.get_name();
    if (param_name.find(name_ + ".") != 0) {
      continue;
    }
    if (param_type == ParameterType::PARAMETER_DOUBLE) {
      if (param_name != name_ + "." + "custom_inscribed_radius" &&
        parameter.as_double() < 0.0)
      {
        RCLCPP_WARN(
        logger_, "The value of parameter '%s' is incorrectly set to %f, "
        "it should be >=0. Ignoring parameter update.",
        param_name.c_str(), parameter.as_double());
        result.successful = false;
      }
    }
  }
  return result;
}
 
void
InflationLayer::updateParametersCallback(
  const std::vector<rclcpp::Parameter> & parameters)
{
  std::lock_guard<Costmap2D::mutex_t> guard(*getMutex());
 
  bool need_cache_recompute = false;
 
  for (const auto & parameter : parameters) {
    const auto & param_type = parameter.get_type();
    const auto & param_name = parameter.get_name();
    if (param_name.find(name_ + ".") != 0) {
      continue;
    }
 
    if (param_type == ParameterType::PARAMETER_DOUBLE) {
      if (param_name == name_ + "." + "inflation_radius" &&
        inflation_radius_ != parameter.as_double())
      {
        inflation_radius_ = parameter.as_double();
        need_reinflation_ = true;
        need_cache_recompute = true;
        setCurrent(false);
      } else if (param_name == name_ + "." + "custom_inscribed_radius" && // NOLINT
        custom_inscribed_radius_ != parameter.as_double())
      {
        custom_inscribed_radius_ = parameter.as_double();
        updateInscribedRadius();
        need_reinflation_ = true;
        need_cache_recompute = true;
      } else if (param_name == name_ + "." + "cost_scaling_factor" && // NOLINT
        getCostScalingFactor() != parameter.as_double())
      {
        cost_scaling_factor_ = parameter.as_double();
        need_reinflation_ = true;
        need_cache_recompute = true;
        setCurrent(false);
      }
    } else if (param_type == ParameterType::PARAMETER_INTEGER) {
      if (param_name == name_ + "." + "num_threads" && // NOLINT
        num_threads_ != parameter.as_int())
      {
        int new_value = parameter.as_int();
#ifdef _OPENMP
        if (new_value < -1) {
          RCLCPP_WARN(
            logger_,
            "Invalid num_threads value %d, must be -1 (auto) or > 0. Ignoring.",
            new_value);
        } else {
          int available_cores = omp_get_max_threads();
          if (new_value > available_cores) {
            RCLCPP_WARN(
              logger_,
              "num_threads=%d exceeds available cores (%d). Ignoring.",
              new_value, available_cores);
          } else {
            num_threads_ = new_value;
            RCLCPP_INFO(
              logger_,
              "Updated num_threads to %d %s",
              num_threads_,
              num_threads_ == -1 ? "(auto)" : "");
          }
        }
#else
        RCLCPP_WARN(
          logger_,
          "num_threads parameter ignored - OpenMP support not available. "
          "Inflation layer will use single thread.");
        num_threads_ = new_value;
#endif
      }
    } else if (param_type == ParameterType::PARAMETER_BOOL) {
      if (param_name == name_ + "." + "enabled" && enabled_ != parameter.as_bool()) {
        enabled_ = parameter.as_bool();
        need_reinflation_ = true;
        setCurrent(false);
      } else if (param_name == name_ + "." + "inflate_unknown" && // NOLINT
        inflate_unknown_ != parameter.as_bool())
      {
        inflate_unknown_ = parameter.as_bool();
        need_reinflation_ = true;
        setCurrent(false);
      } else if (param_name == name_ + "." + "inflate_around_unknown" && // NOLINT
        inflate_around_unknown_ != parameter.as_bool())
      {
        inflate_around_unknown_ = parameter.as_bool();
        need_reinflation_ = true;
        setCurrent(false);
      }
    }
  }
 
  if (need_cache_recompute) {
    matchSize();
  }
}
 
}  // namespace nav2_costmap_2d