inflation_layer.hpp

// 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.
 
 
#ifndef NAV2_COSTMAP_2D__INFLATION_LAYER_HPP_
#define NAV2_COSTMAP_2D__INFLATION_LAYER_HPP_
 
#include <limits>
#include <map>
#include <memory>
#include <mutex>
#include <string>
#include <vector>
#ifdef _OPENMP
#include <omp.h>
#endif
#include <Eigen/Core>
 
#include "rclcpp/node_interfaces/node_parameters_interface.hpp"
#include "rclcpp/parameter.hpp"
#include "nav2_costmap_2d/inflation_layer_interface.hpp"
#include "nav2_costmap_2d/layered_costmap.hpp"
#include "nav2_costmap_2d/distance_transform.hpp"
 
namespace nav2_costmap_2d
{
 
class InflationLayer : public InflationLayerInterface
{
public:
  InflationLayer();
 
  ~InflationLayer();
 
  void onInitialize() override;
 
  void deactivate() override;
 
  void activate() override;
 
  void updateBounds(
    double robot_x, double robot_y, double robot_yaw, double * min_x,
    double * min_y,
    double * max_x,
    double * max_y) override;
  void updateCosts(
    nav2_costmap_2d::Costmap2D & master_grid,
    int min_i, int min_j, int max_i, int max_j) override;
 
  void matchSize() override;
 
  bool isClearable() override {return false;}
 
  void reset() override
  {
    matchSize();
    setCurrent(false);
    need_reinflation_ = true;
  }
 
  inline unsigned char computeCost(double distance) const override
  {
    unsigned char cost = 0;
    if (distance == 0) {
      cost = LETHAL_OBSTACLE;
    } else if (distance * resolution_ <= inscribed_radius_) {
      cost = INSCRIBED_INFLATED_OBSTACLE;
    } else {
      // make sure cost falls off by Euclidean distance
      double factor =
        exp(-1.0 * cost_scaling_factor_ * (distance * resolution_ - inscribed_radius_));
      cost = static_cast<unsigned char>((INSCRIBED_INFLATED_OBSTACLE - 1) * factor);
    }
    return cost;
  }
 
  mutex_t * getMutex() override
  {
    return access_;
  }
 
  double getCostScalingFactor() override
  {
    return cost_scaling_factor_;
  }
 
  double getInflationRadius() override
  {
    return inflation_radius_;
  }
 
protected:
  void 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);
 
  void onFootprintChanged() override;
 
  unsigned int cellDistance(double world_dist)
  {
    return layered_costmap_->getCostmap()->cellDistance(world_dist);
  }
 
  void computeCaches();
 
  int getOptimalThreadCount();
 
  rcl_interfaces::msg::SetParametersResult validateParameterUpdatesCallback(
    const std::vector<rclcpp::Parameter> & parameters);
 
  void updateParametersCallback(const std::vector<rclcpp::Parameter> & parameters);
 
  void updateInscribedRadius();
 
  double inflation_radius_, inscribed_radius_, custom_inscribed_radius_, cost_scaling_factor_;
  bool inflate_unknown_, inflate_around_unknown_;
  unsigned int cell_inflation_radius_;
  int num_threads_;  // Number of OpenMP threads (-1 = auto)
  double resolution_;
 
  // Cost LUT precision: 100 samples per cell provides smooth gradients
  static constexpr int COST_LUT_PRECISION = 100;
  std::vector<unsigned char> cost_lut_;
  double last_min_x_, last_min_y_, last_max_x_, last_max_y_;
 
  // Indicates that the entire costmap should be reinflated next time around.
  bool need_reinflation_;
  mutex_t * access_;
  // Dynamic parameters handler
  rclcpp::node_interfaces::PostSetParametersCallbackHandle::SharedPtr post_set_params_handler_;
  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr on_set_params_handler_;
};
 
}  // namespace nav2_costmap_2d
 
#endif  // NAV2_COSTMAP_2D__INFLATION_LAYER_HPP_